sunnypilot v2026.002.000

version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8

openpilot/sunnypilot/selfdrive/assets/offroad/icon_firehose.svg

@@ -0,0 +1 @@
+<svg fill="#ffffff" version="1.1" id="Capa_1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" viewBox="0 0 900.099 900.1" xml:space="preserve" stroke="#ffffff"><g id="SVGRepo_bgCarrier" stroke-width="0"></g><g id="SVGRepo_tracerCarrier" stroke-linecap="round" stroke-linejoin="round"></g><g id="SVGRepo_iconCarrier"> <g> <path d="M453.95,528.801c109,0,197.7-88.701,197.7-197.701s-88.7-197.7-197.7-197.7s-197.7,88.7-197.7,197.7 C256.25,440.2,344.95,528.801,453.95,528.801z M453.95,223.5c59.4,0,107.7,48.3,107.7,107.7s-48.3,107.7-107.7,107.7 s-107.7-48.3-107.7-107.7S394.549,223.5,453.95,223.5z"></path> <path d="M122.95,677.1h90V558.301c2.2,2.398,4.5,4.699,6.8,7c30.4,30.398,65.8,54.299,105.3,71c40.9,17.299,84.2,26,128.9,26 s88.1-8.801,128.9-26c39.399-16.701,74.899-40.602,105.3-71c2.3-2.301,4.6-4.701,6.8-7V640.1h90v-298c0.1-3.6,0.2-7.3,0.2-10.9 c0-44.7-8.8-88.1-26-128.9c-16.7-39.4-40.601-74.9-71-105.3c-30.4-30.4-65.8-54.3-105.3-71c-40.9-17.3-84.2-26-128.9-26 s-88.1,8.8-128.9,26c-39.399,16.7-74.899,40.6-105.3,71c-30.4,30.4-54.3,65.8-71,105.3c-17.3,40.9-26,84.2-26,128.9 c0,3.7,0.1,7.3,0.2,10.9V677.1L122.95,677.1z M283.35,160.6C328.95,115,389.45,90,453.95,90s125,25.1,170.6,70.6 c45.5,45.5,70.601,106.1,70.601,170.5h-0.2c0,0,0,0,0,0.1c0,63.8-25.3,125.399-70.4,170.601c-45.6,45.5-106.199,70.6-170.6,70.6 s-125-25.1-170.6-70.6c-44.9-45-70.4-107.201-70.4-170.601c0,0,0,0,0-0.1h-0.2C212.75,266.7,237.85,206.1,283.35,160.6z"></path> <path d="M93.25,799.301l42.5,89.398c3.3,7,10.3,11.4,18.1,11.4h14.1h14.5c7.7,0,14.8-4.5,18.1-11.5l42.4-89.699 c1.3-2.701,2.1-5.6,2.1-8.5v-54c0-11-9.3-20.301-20.3-20.301h-11.8h-90h-11.7c-11,0-20,9.1-20,20.201v54.398 C91.25,793.6,91.95,796.6,93.25,799.301z"></path> <path d="M671.25,694.301V738.1c0,11,9,20,20,20l0,0h97.5c11,0,20.101-8.9,20.101-19.9V699.5c0-11-9.301-20.301-20.301-20.301 h-102.3C677.95,679.1,671.25,686,671.25,694.301z"/> </g> </g></svg>

openpilot/sunnypilot/selfdrive/assets/offroad/icon_home.svg

@@ -0,0 +1 @@
+<svg xmlns="http://www.w3.org/2000/svg" width="100" height="100" fill="none"><g clip-path="url(#a)"><path fill="#fff" d="M7.7 98.08c0 1.06.85 1.92 1.92 1.92h28.84V76.92h23.08V100H90.4c1.06 0 1.92-.86 1.92-1.92V53.85H7.69v44.23ZM99.47 42.9l-7.17-6.3V7.7a7.7 7.7 0 0 0-15.39 0v15.4L51.4.68A1.92 1.92 0 0 0 48.68.6l-.08.08L.52 42.72a1.92 1.92 0 0 0 1.38 3.24l13.49.2h82.68a1.92 1.92 0 0 0 1.41-3.25Z"/></g><defs><clipPath id="a"><path fill="#fff" d="M0 0h100v100H0z"/></clipPath></defs></svg>

openpilot/sunnypilot/selfdrive/car/car_list.json

@@ -0,0 +1 @@
+../../../../opendbc_repo/opendbc/sunnypilot/car/car_list.json

openpilot/sunnypilot/selfdrive/car/car_specific.py

@@ -0,0 +1,51 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from openpilot.cereal import log, custom
+from opendbc.car import structs
+
+from opendbc.car.chrysler.values import RAM_DT
+from openpilot.selfdrive.selfdrived.events import Events
+from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+
+EventName = log.OnroadEvent.EventName
+EventNameSP = custom.OnroadEventSP.EventName
+GearShifter = structs.CarState.GearShifter
+
+
+class CarSpecificEventsSP:
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
+    self.CP = CP
+    self.CP_SP = CP_SP
+
+    self.low_speed_alert = False
+
+  def update(self, CS: structs.CarState, events: Events):
+    events_sp = EventsSP()
+
+    if self.CP.brand == 'chrysler':
+      if self.CP.carFingerprint in RAM_DT:
+        # remove belowSteerSpeed event from CarSpecificEvents as RAM_DT uses a different logic
+        if events.has(EventName.belowSteerSpeed):
+          events.remove(EventName.belowSteerSpeed)
+
+        # TODO-SP: use if/elif to have the gear shifter condition takes precedence over the speed condition
+        # TODO-SP: add 1 m/s hysteresis
+        if CS.vEgo >= self.CP.minEnableSpeed:
+          self.low_speed_alert = False
+        if self.CP.minEnableSpeed >= 14.5 and CS.gearShifter != GearShifter.drive:
+          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

openpilot/sunnypilot/selfdrive/car/cruise_ext.py

@@ -0,0 +1,139 @@
+"""
+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 openpilot.cereal import custom
+from opendbc.car.structs import car
+from opendbc.car import structs
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import ACTIVE_STATES as SLA_ACTIVE_STATES
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import compare_cluster_target
+
+ButtonType = car.CarState.ButtonEvent.Type
+SpeedLimitAssistState = custom.LongitudinalPlanSP.SpeedLimit.AssistState
+
+CRUISE_BUTTON_TIMER = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0,
+                       ButtonType.setCruise: 0, ButtonType.resumeCruise: 0,
+                       ButtonType.cancel: 0, ButtonType.mainCruise: 0}
+
+V_CRUISE_MIN = 8
+V_CRUISE_MAX = 145
+V_CRUISE_UNSET = 255
+
+
+def update_manual_button_timers(CS: car.CarState, button_timers: dict[car.CarState.ButtonEvent.Type, int]) -> None:
+  # increment timer for buttons still pressed
+  for k in button_timers:
+    if button_timers[k] > 0:
+      button_timers[k] += 1
+
+  for b in CS.buttonEvents:
+    if b.type.raw in button_timers:
+      # Start/end timer and store current state on change of button pressed
+      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
+
+    self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled")
+    self.short_increment = self.params.get("CustomAccShortPressIncrement", return_default=True)
+    self.long_increment = self.params.get("CustomAccLongPressIncrement", return_default=True)
+
+    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)
+    self.long_increment = self.params.get("CustomAccLongPressIncrement", return_default=True)
+
+  def update_v_cruise_delta(self, long_press: bool, v_cruise_delta: float) -> tuple[bool, float]:
+    if not self.custom_acc_enabled:
+      v_cruise_delta = v_cruise_delta * (5 if long_press else 1)
+      return long_press, v_cruise_delta
+
+    # Apply user-specified multipliers to the base increment
+    short_increment = np.clip(self.short_increment, 1, 10)
+    long_increment = np.clip(self.long_increment, 1, 10)
+
+    actual_increment = long_increment if long_press else short_increment
+    round_to_nearest = actual_increment in (5, 10)
+    v_cruise_delta = v_cruise_delta * actual_increment
+
+    return round_to_nearest, v_cruise_delta
+
+  def get_minimum_set_speed(self, is_metric: bool) -> None:
+    if self.CP_SP.pcmCruiseSpeed:
+      self.v_cruise_min = V_CRUISE_MIN
+      return
+
+    self.v_cruise_min = get_minimum_set_speed(is_metric)
+
+  def update_enabled_state(self, CS: car.CarState, enabled: bool) -> bool:
+    # special enabled state for non pcmCruiseSpeed, unchanged for non pcmCruise
+    if not self.CP_SP.pcmCruiseSpeed:
+      update_manual_button_timers(CS, self.enable_button_timers)
+      button_pressed = any(self.enable_button_timers[k] > 0 for k in self.enable_button_timers)
+
+      if enabled and not self.enabled_prev:
+        self.enabled_prev = not button_pressed
+        enabled = False
+      elif not enabled:
+        self.enabled_prev = enabled
+
+      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

openpilot/sunnypilot/selfdrive/car/cruise_helpers.py

@@ -0,0 +1,51 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from openpilot.cereal import custom
+from opendbc.car.structs import car
+from opendbc.car import structs
+from openpilot.common.params import Params
+
+ButtonType = car.CarState.ButtonEvent.Type
+EventNameSP = custom.OnroadEventSP.EventName
+
+DISTANCE_LONG_PRESS = 50
+
+
+class CruiseHelper:
+  def __init__(self, CP: structs.CarParams):
+    self.CP = CP
+    self.params = Params()
+
+    self.button_frame_counts = {ButtonType.gapAdjustCruise: 0}
+    self._experimental_mode = False
+    self.experimental_mode_switched = False
+
+  def update(self, CS, events, experimental_mode) -> None:
+    if self.CP.openpilotLongitudinalControl:
+      if CS.cruiseState.available:
+        self.update_button_frame_counts(CS)
+
+        # toggle experimental mode once on distance button hold
+        self.update_experimental_mode(events, experimental_mode)
+
+  def update_button_frame_counts(self, CS) -> None:
+    for button in self.button_frame_counts:
+      if self.button_frame_counts[button] > 0:
+        self.button_frame_counts[button] += 1
+
+    for button_event in CS.buttonEvents:
+      button = button_event.type.raw
+      if button in self.button_frame_counts:
+        self.button_frame_counts[button] = int(button_event.pressed)
+
+  def update_experimental_mode(self, events, experimental_mode) -> None:
+    if self.button_frame_counts[ButtonType.gapAdjustCruise] >= DISTANCE_LONG_PRESS and not self.experimental_mode_switched:
+      self._experimental_mode = not experimental_mode
+      self.params.put_bool("ExperimentalMode", self._experimental_mode)
+      events.add(EventNameSP.experimentalModeSwitched)
+      self.experimental_mode_switched = True

openpilot/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py

@@ -0,0 +1,128 @@
+"""
+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.cereal import custom
+from opendbc.car.structs import car
+from opendbc.car import structs, apply_hysteresis
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_CTRL
+from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.helpers import get_minimum_set_speed
+from openpilot.sunnypilot.selfdrive.car.cruise_ext import CRUISE_BUTTON_TIMER, update_manual_button_timers
+
+LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
+State = custom.IntelligentCruiseButtonManagement.IntelligentCruiseButtonManagementState
+SendButtonState = custom.IntelligentCruiseButtonManagement.SendButtonState
+
+ALLOWED_SPEED_THRESHOLD = 1.8  # m/s, ~4 MPH
+HYST_GAP = 0.0  # currently disabled; TODO-SP: might need to be brand-specific
+INACTIVE_TIMER = 0.4
+
+
+SEND_BUTTONS = {
+  State.increasing: SendButtonState.increase,
+  State.decreasing: SendButtonState.decrease,
+}
+
+
+class IntelligentCruiseButtonManagement:
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
+    self.CP = CP
+    self.CP_SP = CP_SP
+
+    self.v_target = 0
+    self.v_cruise_cluster = 0
+    self.v_cruise_min = 0
+    self.cruise_button = SendButtonState.none
+    self.state = State.inactive
+    self.pre_active_timer = 0
+
+    self.is_ready = False
+    self.is_ready_prev = False
+    self.v_target_ms_last = 0.0
+    self.is_metric = False
+
+    self.cruise_button_timers = CRUISE_BUTTON_TIMER
+
+  @property
+  def v_cruise_equal(self) -> bool:
+    return self.v_target == self.v_cruise_cluster
+
+  def update_calculations(self, CS: car.CarState, LP_SP: custom.LongitudinalPlanSP) -> None:
+    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
+
+    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)
+    self.v_cruise_cluster = round(CS.cruiseState.speedCluster * speed_conv)
+
+  def update_state_machine(self) -> custom.IntelligentCruiseButtonManagement.SendButtonState:
+    self.pre_active_timer = max(0, self.pre_active_timer - 1)
+
+    # HOLDING, ACCELERATING, DECELERATING, PRE_ACTIVE
+    if self.state != State.inactive:
+      if not self.is_ready:
+        self.state = State.inactive
+
+      else:
+        # PRE_ACTIVE
+        if self.state == State.preActive:
+          if self.pre_active_timer <= 0:
+            if self.v_cruise_equal:
+              self.state = State.holding
+
+            elif self.v_target > self.v_cruise_cluster:
+              self.state = State.increasing
+
+            elif self.v_target < self.v_cruise_cluster and self.v_cruise_cluster > self.v_cruise_min:
+              self.state = State.decreasing
+
+        # HOLDING
+        elif self.state == State.holding:
+          if not self.v_cruise_equal:
+            self.state = State.preActive
+
+        # ACCELERATING
+        elif self.state == State.increasing:
+          if self.v_target <= self.v_cruise_cluster:
+            self.state = State.holding
+
+        # DECELERATING
+        elif self.state == State.decreasing:
+          if self.v_target >= self.v_cruise_cluster or self.v_cruise_cluster <= self.v_cruise_min:
+            self.state = State.holding
+
+    # INACTIVE
+    elif self.state == State.inactive:
+      if self.is_ready and not self.is_ready_prev:
+        self.pre_active_timer = int(INACTIVE_TIMER / DT_CTRL)
+        self.state = State.preActive
+
+    send_button = SEND_BUTTONS.get(self.state, SendButtonState.none)
+
+    return send_button
+
+  def update_readiness(self, CS: car.CarState, CC: car.CarControl) -> None:
+    update_manual_button_timers(CS, self.cruise_button_timers)
+
+    ready = CC.enabled and not CC.cruiseControl.override and not CC.cruiseControl.cancel and not CC.cruiseControl.resume
+    button_pressed = any(self.cruise_button_timers[k] > 0 for k in self.cruise_button_timers)
+
+    self.is_ready = ready and not button_pressed
+
+  def run(self, CS: car.CarState, CC: car.CarControl, LP_SP: custom.LongitudinalPlanSP, is_metric: bool) -> None:
+    if self.CP_SP.pcmCruiseSpeed:
+      return
+
+    self.is_metric = is_metric
+
+    self.update_calculations(CS, LP_SP)
+    self.update_readiness(CS, CC)
+
+    self.cruise_button = self.update_state_machine()
+
+    self.is_ready_prev = self.is_ready

openpilot/sunnypilot/selfdrive/car/intelligent_cruise_button_management/helpers.py

@@ -0,0 +1,8 @@
+"""
+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.
+"""
+def get_minimum_set_speed(is_metric: bool) -> int:
+  return 30 if is_metric else 20

openpilot/sunnypilot/selfdrive/car/interfaces.py

@@ -0,0 +1,134 @@
+"""
+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 typing import Any
+
+from opendbc.car import structs
+from opendbc.car.interfaces import CarInterfaceBase
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import get_nn_model_path
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import set_speed_limit_assist_availability
+
+import openpilot.system.sentry as sentry
+
+from openpilot.sunnypilot.sunnylink.statsd import STATSLOGSP
+
+
+def log_fingerprint(CP: structs.CarParams) -> None:
+  if CP.carFingerprint == "MOCK":
+    sentry.capture_fingerprint_mock()
+  else:
+    sentry.capture_fingerprint(CP.carFingerprint, CP.brand)
+
+
+def _enforce_torque_lateral_control(CP: structs.CarParams, params: Params = None, enabled: bool = False) -> bool:
+  if params is None:
+    params = Params()
+
+  if CP.steerControlType != structs.CarParams.SteerControlType.angle:
+    enabled = params.get_bool("EnforceTorqueControl")
+
+  return enabled
+
+
+def _initialize_neural_network_lateral_control(CP: structs.CarParams, CP_SP: structs.CarParamsSP,
+                                               params: Params = None, enabled: bool = False) -> bool:
+  if params is None:
+    params = Params()
+
+  nnlc_model_path, nnlc_model_name, exact_match = get_nn_model_path(CP)
+
+  if nnlc_model_name == "MOCK":
+    cloudlog.error({"nnlc event": "car doesn't match any Neural Network model"})
+
+  if nnlc_model_name != "MOCK" and CP.steerControlType != structs.CarParams.SteerControlType.angle:
+    enabled = params.get_bool("NeuralNetworkLateralControl")
+
+  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:
+    params = Params()
+
+  icbm_enabled = params.get_bool("IntelligentCruiseButtonManagement")
+  if icbm_enabled and CP_SP.intelligentCruiseButtonManagementAvailable and not CP.openpilotLongitudinalControl:
+    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 _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params = None) -> None:
+  if params is None:
+    params = Params()
+
+  if CP.steerControlType == structs.CarParams.SteerControlType.angle:
+    cloudlog.warning("SteerControlType is angle, cleaning up params")
+    params.remove("NeuralNetworkLateralControl")
+    params.remove("EnforceTorqueControl")
+
+  if not CP_SP.intelligentCruiseButtonManagementAvailable or CP.openpilotLongitudinalControl:
+    cloudlog.warning("ICBM not available or openpilot Longitudinal Control enabled, cleaning up params")
+    params.remove("IntelligentCruiseButtonManagement")
+
+  if not CP.openpilotLongitudinalControl and CP_SP.pcmCruiseSpeed:
+    cloudlog.warning("openpilot Longitudinal Control and ICBM not available, cleaning up params")
+    params.remove("DynamicExperimentalControl")
+    params.remove("CustomAccIncrementsEnabled")
+    params.remove("SmartCruiseControlVision")
+    params.remove("SmartCruiseControlMap")
+
+  set_speed_limit_assist_availability(CP, CP_SP, params)
+
+
+def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None:
+  enforce_torque = _enforce_torque_lateral_control(CI.CP, params)
+  nnlc_enabled = _initialize_neural_network_lateral_control(CI.CP, CI.CP_SP, params)
+  _initialize_intelligent_cruise_button_management(CI.CP, CI.CP_SP, params)
+  _initialize_torque_lateral_control(CI, CI.CP, enforce_torque, nnlc_enabled)
+  _cleanup_unsupported_params(CI.CP, CI.CP_SP)
+
+  try:
+    STATSLOGSP.raw('sunnypilot.car_params', CI.CP.to_dict())
+  except RuntimeError:
+    pass  # to_dict fails on macOS due to library issues.
+  # STATSLOGSP.raw('sunnypilot_params.car_params_sp', CP_SP.to_dict()) # https://github.com/sunnypilot/opendbc/pull/361
+
+
+def initialize_params(params) -> list[dict[str, Any]]:
+  keys: list = []
+
+  # hyundai
+  keys.extend([
+    "HyundaiLongitudinalTuning",
+  ])
+
+  # subaru
+  keys.extend([
+    "SubaruStopAndGo",
+    "SubaruStopAndGoManualParkingBrake",
+  ])
+
+  # tesla
+  keys.extend([
+    "TeslaCoopSteering",
+  ])
+
+  # toyota
+  keys.extend([
+    "ToyotaEnforceStockLongitudinal",
+    "ToyotaStopAndGoHack",
+  ])
+
+  return [{k: params.get(k, return_default=True)} for k in keys]

openpilot/sunnypilot/selfdrive/car/sync_sunnylink_params.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python3
+"""
+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 json
+import os
+
+from openpilot.common.basedir import BASEDIR
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+
+CAR_LIST_JSON_OUT = os.path.join(BASEDIR, "openpilot", "sunnypilot", "selfdrive", "car", "car_list.json")
+
+
+def update_car_list_param():
+  with open(CAR_LIST_JSON_OUT) as f:
+    current_car_list = json.load(f)
+
+  params = Params()
+  if params.get("CarList") != current_car_list:
+    params.put("CarList", current_car_list, block=True)
+    cloudlog.warning("Updated CarList param with latest platform list")
+  else:
+    cloudlog.warning("CarList param is up to date, no need to update")
+
+
+if __name__ == "__main__":
+  update_car_list_param()

openpilot/sunnypilot/selfdrive/car/tests/test_cruise_mode.py

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

openpilot/sunnypilot/selfdrive/car/tests/test_custom_cruise.py

@@ -0,0 +1,150 @@
+import pytest
+
+from opendbc.car.structs import car
+from openpilot.common.constants import CV
+from openpilot.common.parameterized import parameterized_class
+from openpilot.common.params import Params
+from openpilot.selfdrive.car.cruise import V_CRUISE_INITIAL
+from openpilot.selfdrive.car.tests.test_cruise_speed import TestVCruiseHelper
+
+ButtonEvent = car.CarState.ButtonEvent
+ButtonType = car.CarState.ButtonEvent.Type
+
+
+# TODO: test pcmCruise and pcmCruiseSpeed
+@parameterized_class(('pcm_cruise', 'pcm_cruise_speed'), [(False, True)])
+class TestCustomAccIncrements(TestVCruiseHelper):
+  def setup_method(self):
+    TestVCruiseHelper.setup_method(self)
+    self.params = Params()
+    self.reset_custom_params()
+
+  def reset_custom_params(self) -> None:
+    """Reset to default custom ACC parameters"""
+    self.params.put_bool("CustomAccIncrementsEnabled", False, block=True)
+    self.params.put("CustomAccShortPressIncrement", 1, block=True)
+    self.params.put("CustomAccLongPressIncrement", 5, block=True)
+    self.v_cruise_helper.read_custom_set_speed_params()
+
+  def press_button_short(self, button_type: car.CarState.ButtonEvent.Type) -> None:
+    """Simulate a short button press (press + release)"""
+    CS = car.CarState(cruiseState={"available": True})
+    CS.buttonEvents = [ButtonEvent(type=button_type, pressed=True)]
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True)
+
+    CS.buttonEvents = [ButtonEvent(type=button_type, pressed=False)]
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True)
+
+  def press_button_long(self, button_type: car.CarState.ButtonEvent.Type) -> None:
+    """Simulate a long button press (50+ frames)"""
+    CS = car.CarState(cruiseState={"available": True})
+    CS.buttonEvents = [ButtonEvent(type=button_type, pressed=True)]
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True)
+
+    # Hold for 50 frames to trigger long press
+    CS.buttonEvents = []
+    for _ in range(50):
+      self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True)
+
+    CS.buttonEvents = [ButtonEvent(type=button_type, pressed=False)]
+    self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True)
+
+  def set_custom_increments(self, enabled: bool, short_inc: int, long_inc: int) -> None:
+    """Set custom ACC increment parameters"""
+    self.params.put_bool("CustomAccIncrementsEnabled", enabled, block=True)
+    self.params.put("CustomAccShortPressIncrement", short_inc, block=True)
+    self.params.put("CustomAccLongPressIncrement", long_inc, block=True)
+    self.v_cruise_helper.read_custom_set_speed_params()
+
+  def test_default_behavior_when_disabled(self):
+    """Test that default increments are used when custom ACC is disabled"""
+    self.set_custom_increments(enabled=False, short_inc=5, long_inc=10)
+    self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False)
+
+    initial_speed = self.v_cruise_helper.v_cruise_kph
+
+    # Short press should increment by 1 (default)
+    self.press_button_short(ButtonType.accelCruise)
+    assert self.v_cruise_helper.v_cruise_kph == initial_speed + 1
+
+  @pytest.mark.parametrize("increment", (1, 2, 3, 4, 5, 6, 7, 8, 9, 10))
+  def test_custom_short_press_increments(self, increment):
+    """Test custom short press increments (1-10)"""
+    self.set_custom_increments(enabled=True, short_inc=increment, long_inc=5)
+    self.enable(50 * CV.KPH_TO_MS, False, False)
+
+    initial_speed = self.v_cruise_helper.v_cruise_kph
+    self.press_button_short(ButtonType.accelCruise)
+
+    if increment in (5, 10):
+      # Should round to nearest increment
+      expected_speed = ((initial_speed // increment) + 1) * increment
+    else:
+      expected_speed = initial_speed + increment
+
+    assert self.v_cruise_helper.v_cruise_kph == expected_speed
+
+  @pytest.mark.parametrize("increment", (1, 5, 10))
+  def test_custom_long_press_increments(self, increment):
+    """Test custom long press increments (1, 5, 10)"""
+    self.set_custom_increments(enabled=True, short_inc=1, long_inc=increment)
+    self.enable(50 * CV.KPH_TO_MS, False, False)
+
+    initial_speed = self.v_cruise_helper.v_cruise_kph
+    self.press_button_long(ButtonType.accelCruise)
+
+    if increment in (5, 10):
+      # Should round to nearest increment
+      expected_speed = ((initial_speed // increment) + 1) * increment
+    else:
+      expected_speed = initial_speed + increment
+
+    assert self.v_cruise_helper.v_cruise_kph == expected_speed
+
+  @pytest.mark.parametrize("button_type", [ButtonType.accelCruise, ButtonType.decelCruise])
+  def test_accel_decel_symmetry(self, button_type):
+    """Test that acceleration and deceleration work symmetrically"""
+    self.set_custom_increments(enabled=True, short_inc=3, long_inc=5)
+    self.enable(50 * CV.KPH_TO_MS, False, False)
+
+    initial_speed = self.v_cruise_helper.v_cruise_kph
+    self.press_button_short(button_type)
+
+    expected_change = 3 if button_type == ButtonType.accelCruise else -3
+    assert self.v_cruise_helper.v_cruise_kph == initial_speed + expected_change
+
+  def test_rounding_behavior(self):
+    """Test rounding behavior for 5 and 10 increments"""
+    test_cases = [
+      (47, 5, 50),   # 47 -> 50 (round up to next 5)
+      (45, 5, 50),   # 45 -> 50 (already at 5, increment by 5)
+      (43, 10, 50),  # 43 -> 50 (round up to next 10)
+      (40, 10, 50),  # 40 -> 50 (already at 10, increment by 10)
+    ]
+
+    for initial, increment, expected in test_cases:
+      self.set_custom_increments(enabled=True, short_inc=increment, long_inc=increment)
+      self.reset_cruise_speed_state()
+      self.enable(initial * CV.KPH_TO_MS, False, False)
+
+      self.press_button_short(ButtonType.accelCruise)
+      assert self.v_cruise_helper.v_cruise_kph == expected
+
+  def test_invalid_values_fallback(self):
+    """Test that invalid values fallback to safe defaults"""
+    # Test invalid short increment
+    self.set_custom_increments(enabled=True, short_inc=-1, long_inc=5)
+    self.enable(50 * CV.KPH_TO_MS, False, False)
+
+    initial_speed = self.v_cruise_helper.v_cruise_kph
+    self.press_button_short(ButtonType.accelCruise)
+    assert self.v_cruise_helper.v_cruise_kph == initial_speed + 1  # Should fallback to 1
+
+    # Test invalid long increment
+    self.reset_cruise_speed_state()
+    self.set_custom_increments(enabled=True, short_inc=1, long_inc=99)
+    self.enable(50 * CV.KPH_TO_MS, False, False)
+
+    initial_speed = self.v_cruise_helper.v_cruise_kph
+    self.press_button_long(ButtonType.accelCruise)
+    assert self.v_cruise_helper.v_cruise_kph == initial_speed + 10  # Should fallback to 10

openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -0,0 +1,119 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import time
+
+import openpilot.cereal.messaging as messaging
+from openpilot.cereal import log, custom
+
+from opendbc.car import structs
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
+from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
+from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
+from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_v0 import LatControlTorque as LatControlTorqueV0
+
+
+class ControlsExt(ModelStateBase):
+  def __init__(self, CP: structs.CarParams, params: Params):
+    ModelStateBase.__init__(self)
+    self.CP = CP
+    self.params = params
+    self._param_update_time: float = 0.0
+    self.blinker_pause_lateral = BlinkerPauseLateral()
+
+    cloudlog.info("controlsd_ext is waiting for CarParamsSP")
+    self.CP_SP = messaging.log_from_bytes(params.get("CarParamsSP", block=True), custom.CarParamsSP)
+    cloudlog.info("controlsd_ext got CarParamsSP")
+
+    self.sm_services_ext = ['radarState', 'selfdriveStateSP']
+    self.pm_services_ext = ['carControlSP']
+
+  def initialize_lateral_control(self, lac, CI, dt):
+    enforce_torque_control = self.params.get_bool("EnforceTorqueControl")
+    torque_versions = self.params.get("TorqueControlTune")
+    if not enforce_torque_control:
+      if self.CP.lateralTuning.which() == 'torque':
+        return LatControlTorqueV0(self.CP, self.CP_SP, CI, dt)  # FIXME-SP: revert when upstream fixes tuning issues with v1
+      return lac
+
+    if torque_versions == 0.0:  # v0
+      return LatControlTorqueV0(self.CP, self.CP_SP, CI, dt)
+    else:
+      return lac
+
+  def get_params_sp(self, sm: messaging.SubMaster) -> None:
+    if time.monotonic() - self._param_update_time > PARAMS_UPDATE_PERIOD:
+      self.blinker_pause_lateral.get_params()
+
+      if self.CP.lateralTuning.which() == 'torque':
+        self.lat_delay = get_lat_delay(self.params, sm["liveDelay"].lateralDelay)
+
+      self._param_update_time = time.monotonic()
+
+  def get_lat_active(self, sm: messaging.SubMaster) -> bool:
+    if self.blinker_pause_lateral.update(sm['carState']):
+      return False
+
+    ss_sp = sm['selfdriveStateSP']
+    if ss_sp.mads.available:
+      return bool(ss_sp.mads.active)
+
+    # MADS not available, use stock state to engage
+    return bool(sm['selfdriveState'].active)
+
+  @staticmethod
+  def get_lead_data(_lead, src: log.RadarState.LeadData) -> None:
+    _lead.dRel = src.dRel
+    _lead.yRel = src.yRel
+    _lead.vRel = src.vRel
+    _lead.aRel = src.deprecated.aRel
+    _lead.vLead = src.vLead
+    _lead.dPath = src.deprecated.dPath
+    _lead.vLat = src.deprecated.vLat
+    _lead.vLeadK = src.vLeadK
+    _lead.aLeadK = src.aLeadK
+    _lead.fcw = src.deprecated.fcw
+    _lead.status = src.present
+    _lead.aLeadTau = src.aLeadTau
+    _lead.modelProb = src.modelProb
+    _lead.radar = src.radar
+    _lead.radarTrackId = src.radarTrackId
+
+  def state_control_ext(self, sm: messaging.SubMaster) -> custom.CarControlSP:
+    CC_SP = custom.CarControlSP.new_message()
+
+    self.get_lead_data(CC_SP.leadOne, sm['radarState'].leadOne)
+    self.get_lead_data(CC_SP.leadTwo, sm['radarState'].leadTwo)
+
+    # MADS state
+    mads_src = sm['selfdriveStateSP'].mads
+    CC_SP.mads.state = mads_src.state
+    CC_SP.mads.enabled = mads_src.enabled
+    CC_SP.mads.active = mads_src.active
+    CC_SP.mads.available = mads_src.available
+
+    # ICBM state
+    icbm_src = sm['selfdriveStateSP'].intelligentCruiseButtonManagement
+    CC_SP.intelligentCruiseButtonManagement.state = icbm_src.state
+    CC_SP.intelligentCruiseButtonManagement.sendButton = icbm_src.sendButton
+    CC_SP.intelligentCruiseButtonManagement.vTarget = icbm_src.vTarget
+
+    return CC_SP
+
+  @staticmethod
+  def publish_ext(CC_SP: custom.CarControlSP, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
+    cc_sp_send = messaging.new_message('carControlSP')
+    cc_sp_send.valid = sm['carState'].canValid
+    cc_sp_send.carControlSP = CC_SP
+
+    pm.send('carControlSP', cc_sp_send)
+
+  def run_ext(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
+    CC_SP = self.state_control_ext(sm)
+    self.publish_ext(CC_SP, sm, pm)

openpilot/sunnypilot/selfdrive/controls/lib/auto_lane_change.py

@@ -0,0 +1,112 @@
+"""
+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.cereal import log
+
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+
+
+class AutoLaneChangeMode:
+  OFF = -1
+  NUDGE = 0  # default
+  NUDGELESS = 1
+  HALF_SECOND = 2
+  ONE_SECOND = 3
+  TWO_SECONDS = 4
+  THREE_SECONDS = 5
+
+
+AUTO_LANE_CHANGE_TIMER = {
+  AutoLaneChangeMode.OFF: 0.0,            # Off
+  AutoLaneChangeMode.NUDGE: 0.0,          # Nudge
+  AutoLaneChangeMode.NUDGELESS: 0.05,     # Nudgeless
+  AutoLaneChangeMode.HALF_SECOND: 0.5,    # 0.5-second delay
+  AutoLaneChangeMode.ONE_SECOND: 1.0,     # 1-second delay
+  AutoLaneChangeMode.TWO_SECONDS: 2.0,    # 2-second delay
+  AutoLaneChangeMode.THREE_SECONDS: 3.0,  # 3-second delay
+}
+
+ONE_SECOND_DELAY = -1
+
+
+class AutoLaneChangeController:
+  def __init__(self, desire_helper):
+    self.DH = desire_helper
+    self.params = Params()
+
+    self.lane_change_wait_timer = 0.0
+    self.param_read_counter = 0
+    self.lane_change_delay = 0.0
+
+    self.lane_change_set_timer = self.params.get("AutoLaneChangeTimer", return_default=True)
+    self.lane_change_bsm_delay = False
+
+    self.prev_brake_pressed = False
+    self.auto_lane_change_allowed = False
+    self.prev_lane_change = False
+
+    self.read_params()
+
+  def reset(self) -> None:
+    # Auto reset if parent state indicates we should
+    if self.DH.lane_change_state == log.LaneChangeState.off and \
+       self.DH.lane_change_direction == log.LaneChangeDirection.none:
+      self.lane_change_wait_timer = 0.0
+      self.prev_brake_pressed = False
+      self.prev_lane_change = False
+
+  def read_params(self) -> None:
+    self.lane_change_bsm_delay = self.params.get_bool("AutoLaneChangeBsmDelay")
+    self.lane_change_set_timer = self.params.get("AutoLaneChangeTimer", return_default=True)
+
+  def update_params(self) -> None:
+    if self.param_read_counter % 50 == 0:
+      self.read_params()
+    self.param_read_counter += 1
+
+  def update_lane_change_timers(self, blindspot_detected: bool) -> None:
+    self.lane_change_delay = AUTO_LANE_CHANGE_TIMER.get(self.lane_change_set_timer,
+                                                        AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGE])
+
+    self.lane_change_wait_timer += DT_MDL
+
+    if self.lane_change_bsm_delay and blindspot_detected and self.lane_change_delay > 0:
+      if self.lane_change_delay == AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGELESS]:
+        self.lane_change_wait_timer = ONE_SECOND_DELAY
+      else:
+        self.lane_change_wait_timer = self.lane_change_delay + ONE_SECOND_DELAY
+
+  def update_allowed(self) -> bool:
+    # Auto lane change allowed if:
+    # 1. A valid delay is set (non-zero)
+    # 2. Brake wasn't previously pressed
+    # 3. We've waited long enough
+
+    if self.lane_change_set_timer in (AutoLaneChangeMode.OFF, AutoLaneChangeMode.NUDGE):
+      return False
+
+    if self.prev_brake_pressed:
+      return False
+
+    if self.prev_lane_change:
+      return False
+
+    return bool(self.lane_change_wait_timer > self.lane_change_delay)
+
+  def update_lane_change(self, blindspot_detected: bool, brake_pressed: bool) -> None:
+    if brake_pressed and not self.prev_brake_pressed:
+      self.prev_brake_pressed = brake_pressed
+
+    self.update_lane_change_timers(blindspot_detected)
+
+    self.auto_lane_change_allowed = self.update_allowed()
+
+  def update_state(self):
+    if self.DH.lane_change_state == log.LaneChangeState.laneChangeStarting:
+      self.prev_lane_change = True
+
+    self.reset()

openpilot/sunnypilot/selfdrive/controls/lib/blinker_pause_lateral.py

@@ -0,0 +1,44 @@
+"""
+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 opendbc.car.structs import car
+
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+
+
+class BlinkerPauseLateral:
+  def __init__(self):
+    self.params = Params()
+
+    self.enabled = self.params.get_bool("BlinkerPauseLateralControl")
+    self.is_metric = self.params.get_bool("IsMetric")
+    self.min_speed = 0
+    self.reengage_delay = 0
+    self.blinker_off_timer = 0.0
+
+  def get_params(self) -> None:
+    self.enabled = self.params.get_bool("BlinkerPauseLateralControl")
+    self.is_metric = self.params.get_bool("IsMetric")
+    self.min_speed = self.params.get("BlinkerMinLateralControlSpeed", return_default=True)
+    self.reengage_delay = self.params.get("BlinkerLateralReengageDelay", return_default=True)
+
+  def update(self, CS: car.CarState, DT_CTRL: float = 0.01) -> bool:
+    if not self.enabled:
+      return False
+
+    one_blinker = CS.leftBlinker != CS.rightBlinker
+    speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
+    min_speed_ms = self.min_speed * speed_factor
+
+    below_speed = CS.vEgo < min_speed_ms
+
+    if one_blinker and below_speed:
+      self.blinker_off_timer = self.reengage_delay
+    elif self.blinker_off_timer > 0:
+      self.blinker_off_timer -= DT_CTRL
+
+    return bool((one_blinker and below_speed) or self.blinker_off_timer > 0)

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

@@ -0,0 +1,17 @@
+class WMACConstants:
+  # Lead detection parameters
+  LEAD_WINDOW_SIZE = 6  # Stable detection window
+  LEAD_PROB = 0.45  # Balanced threshold for lead detection
+
+  # Slow down detection parameters
+  SLOW_DOWN_WINDOW_SIZE = 5  # Responsive but stable
+  SLOW_DOWN_PROB = 0.3  # Balanced threshold for slow down scenarios
+
+  # Optimized slow down distance curve - smooth and progressive
+  SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
+  SLOW_DOWN_DIST = [32., 46., 64., 86., 108., 130., 145., 165.]
+
+  # Slowness detection parameters
+  SLOWNESS_WINDOW_SIZE = 10  # Stable slowness detection
+  SLOWNESS_PROB = 0.55  # Clear threshold for slowness
+  SLOWNESS_CRUISE_OFFSET = 1.025  # Conservative cruise speed offset

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

@@ -0,0 +1,388 @@
+"""
+Copyright (c) 2021-, rav4kumar, 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.
+"""
+# Version = 2025-6-30
+
+from openpilot.cereal import messaging
+from opendbc.car import structs
+from numpy import interp
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
+from typing import Literal
+
+# d-e2e, from modeldata.h
+TRAJECTORY_SIZE = 33
+SET_MODE_TIMEOUT = 15
+
+# Define the valid mode types
+ModeType = Literal['acc', 'blended']
+
+
+class SmoothKalmanFilter:
+  """Enhanced Kalman filter with smoothing for stable decision making."""
+
+  def __init__(self, initial_value=0, measurement_noise=0.1, process_noise=0.01,
+               alpha=1.0, smoothing_factor=0.85):
+    self.x = initial_value
+    self.P = 1.0
+    self.R = measurement_noise
+    self.Q = process_noise
+    self.alpha = alpha
+    self.smoothing_factor = smoothing_factor
+    self.initialized = False
+    self.history = []
+    self.max_history = 10
+    self.confidence = 0.0
+
+  def add_data(self, measurement):
+    if len(self.history) >= self.max_history:
+      self.history.pop(0)
+    self.history.append(measurement)
+
+    if not self.initialized:
+      self.x = measurement
+      self.initialized = True
+      self.confidence = 0.1
+      return
+
+    self.P = self.alpha * self.P + self.Q
+
+    K = self.P / (self.P + self.R)
+    effective_K = K * (1.0 - self.smoothing_factor) + self.smoothing_factor * 0.1
+
+    innovation = measurement - self.x
+    self.x = self.x + effective_K * innovation
+    self.P = (1 - effective_K) * self.P
+
+    if abs(innovation) < 0.1:
+      self.confidence = min(1.0, self.confidence + 0.05)
+    else:
+      self.confidence = max(0.1, self.confidence - 0.02)
+
+  def get_value(self):
+    return self.x if self.initialized else None
+
+  def get_confidence(self):
+    return self.confidence
+
+  def reset_data(self):
+    self.initialized = False
+    self.history = []
+    self.confidence = 0.0
+
+
+class ModeTransitionManager:
+  """Manages smooth transitions between driving modes with hysteresis."""
+
+  def __init__(self):
+    self.current_mode: ModeType = 'acc'
+    self.mode_confidence = {'acc': 1.0, 'blended': 0.0}
+    self.transition_timeout = 0
+    self.min_mode_duration = 10
+    self.mode_duration = 0
+    self.emergency_override = False
+
+  def request_mode(self, mode: ModeType, confidence: float = 1.0, emergency: bool = False):
+    # Emergency override for critical situations (stops, collisions)
+    if emergency:
+      self.emergency_override = True
+      self.current_mode = mode
+      self.transition_timeout = SET_MODE_TIMEOUT
+      self.mode_duration = 0
+      return
+
+    self.mode_confidence[mode] = min(1.0, self.mode_confidence[mode] + 0.1 * confidence)
+    for m in self.mode_confidence:
+      if m != mode:
+        self.mode_confidence[m] = max(0.0, self.mode_confidence[m] - 0.05)
+
+    # Require minimum duration in current mode (unless emergency)
+    if self.mode_duration < self.min_mode_duration and not self.emergency_override:
+      return
+
+    # Hysteresis: higher threshold for mode changes
+    confidence_threshold = 0.6 if mode != self.current_mode else 0.3  # Lower threshold for faster response
+
+    if self.mode_confidence[mode] > confidence_threshold:
+      if mode != self.current_mode and self.transition_timeout == 0:
+        self.transition_timeout = SET_MODE_TIMEOUT
+        self.current_mode = mode
+        self.mode_duration = 0
+
+  def update(self):
+    if self.transition_timeout > 0:
+      self.transition_timeout -= 1
+    self.mode_duration += 1
+
+    # Reset emergency override after some time
+    if self.emergency_override and self.mode_duration > 20:
+      self.emergency_override = False
+
+    # Gradual confidence decay
+    for mode in self.mode_confidence:
+      self.mode_confidence[mode] *= 0.98
+
+  def get_mode(self) -> ModeType:
+    return self.current_mode
+
+
+class DynamicExperimentalController:
+  def __init__(self, CP: structs.CarParams, mpc, params=None):
+    self._CP = CP
+    self._mpc = mpc
+    self._params = params or Params()
+    self._enabled: bool = self._params.get_bool("DynamicExperimentalControl")
+    self._active: bool = False
+    self._frame: int = 0
+    self._urgency = 0.0
+
+    self._mode_manager = ModeTransitionManager()
+
+    # Smooth filters for stable decision making with faster response for critical scenarios
+    self._lead_filter = SmoothKalmanFilter(
+      measurement_noise=0.15,
+      process_noise=0.05,
+      alpha=1.02,
+      smoothing_factor=0.8
+    )
+
+    self._slow_down_filter = SmoothKalmanFilter(
+      measurement_noise=0.1,
+      process_noise=0.1,
+      alpha=1.05,
+      smoothing_factor=0.7
+    )
+
+    self._slowness_filter = SmoothKalmanFilter(
+      measurement_noise=0.1,
+      process_noise=0.06,
+      alpha=1.015,
+      smoothing_factor=0.92
+    )
+
+    self._mpc_fcw_filter = SmoothKalmanFilter(
+      measurement_noise=0.2,
+      process_noise=0.1,
+      alpha=1.1,
+      smoothing_factor=0.5
+    )
+    self._has_lead_filtered = False
+    self._has_slow_down = False
+    self._has_slowness = False
+    self._has_mpc_fcw = False
+    self._v_ego_kph = 0.0
+    self._v_cruise_kph = 0.0
+    self._has_standstill = False
+    self._mpc_fcw_crash_cnt = 0
+    self._standstill_count = 0
+    # debug
+    self._endpoint_x = float('inf')
+    self._expected_distance = 0.0
+    self._trajectory_valid = False
+
+  def _read_params(self) -> None:
+    if self._frame % int(1. / DT_MDL) == 0:
+      self._enabled = self._params.get_bool("DynamicExperimentalControl")
+
+  def mode(self) -> str:
+    return self._mode_manager.get_mode()
+
+  def enabled(self) -> bool:
+    return self._enabled
+
+  def active(self) -> bool:
+    return self._active
+
+  def set_mpc_fcw_crash_cnt(self) -> None:
+    """Set MPC FCW crash count"""
+    self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
+
+  def _update_calculations(self, sm: messaging.SubMaster) -> None:
+    car_state = sm['carState']
+    lead_one = sm['radarState'].leadOne
+    md = sm['modelV2']
+
+    self._v_ego_kph = car_state.vEgo * 3.6
+    self._v_cruise_kph = car_state.vCruise
+    self._has_standstill = car_state.standstill
+
+    # standstill detection
+    if self._has_standstill:
+      self._standstill_count = min(20, self._standstill_count + 1)
+    else:
+      self._standstill_count = max(0, self._standstill_count - 1)
+
+    # Lead detection
+    self._lead_filter.add_data(float(lead_one.present))
+    lead_value = self._lead_filter.get_value() or 0.0
+    self._has_lead_filtered = lead_value > WMACConstants.LEAD_PROB
+
+    # MPC FCW detection
+    fcw_filtered_value = self._mpc_fcw_filter.get_value() or 0.0
+    self._mpc_fcw_filter.add_data(float(self._mpc_fcw_crash_cnt > 0))
+    self._has_mpc_fcw = fcw_filtered_value > 0.5
+
+    # Slow down detection
+    self._calculate_slow_down(md)
+
+    # Slowness detection
+    if not (self._standstill_count > 5) and not self._has_slow_down:
+      current_slowness = float(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET))
+      self._slowness_filter.add_data(current_slowness)
+      slowness_value = self._slowness_filter.get_value() or 0.0
+
+      # Hysteresis for slowness
+      threshold = WMACConstants.SLOWNESS_PROB * (0.8 if self._has_slowness else 1.1)
+      self._has_slowness = slowness_value > threshold
+
+  def _calculate_slow_down(self, md):
+    """Calculate urgency based on trajectory endpoint vs expected distance."""
+
+    # Reset to safe defaults
+    urgency = 0.0
+    self._endpoint_x = float('inf')
+    self._trajectory_valid = False
+
+    #Require exact trajectory size
+    position_valid = len(md.position.x) == TRAJECTORY_SIZE
+    orientation_valid = len(md.orientation.x) == TRAJECTORY_SIZE
+
+    if not (position_valid and orientation_valid):
+      # Invalid trajectory - this itself might indicate a stop scenario
+      # Apply moderate urgency for incomplete trajectories at speed
+      if self._v_ego_kph > 20.0:
+        urgency = 0.3
+
+      self._slow_down_filter.add_data(urgency)
+      urgency_filtered = self._slow_down_filter.get_value() or 0.0
+      self._has_slow_down = urgency_filtered > WMACConstants.SLOW_DOWN_PROB
+      self._urgency = urgency_filtered
+      return
+
+    # We have a valid full trajectory
+    self._trajectory_valid = True
+
+    # Use the exact endpoint (33rd point, index 32)
+    endpoint_x = md.position.x[TRAJECTORY_SIZE - 1]
+    self._endpoint_x = endpoint_x
+
+    # Get expected distance based on current speed using tuned constants
+    expected_distance = interp(self._v_ego_kph,
+                               WMACConstants.SLOW_DOWN_BP,
+                               WMACConstants.SLOW_DOWN_DIST)
+    self._expected_distance = expected_distance
+
+    # Calculate urgency based on trajectory shortage
+    if endpoint_x < expected_distance:
+      shortage = expected_distance - endpoint_x
+      shortage_ratio = shortage / expected_distance
+
+      # Base urgency on shortage ratio
+      urgency = min(1.0, shortage_ratio * 2.0)
+
+      # Increase urgency for very short trajectories (imminent stops)
+      critical_distance = expected_distance * 0.3
+      if endpoint_x < critical_distance:
+        urgency = min(1.0, urgency * 2.0)
+
+      # Speed-based urgency adjustment
+      if self._v_ego_kph > 25.0:
+        speed_factor = 1.0 + (self._v_ego_kph - 25.0) / 80.0
+        urgency = min(1.0, urgency * speed_factor)
+
+    # Apply filtering but with less smoothing for stops
+    self._slow_down_filter.add_data(urgency)
+    urgency_filtered = self._slow_down_filter.get_value() or 0.0
+
+    # Update state with lower threshold for better stop detection
+    self._has_slow_down = urgency_filtered > (WMACConstants.SLOW_DOWN_PROB * 0.8)
+    self._urgency = urgency_filtered
+
+  def _radarless_mode(self) -> None:
+    """Radarless mode decision logic with emergency handling."""
+
+    # EMERGENCY: MPC FCW - immediate blended mode
+    if self._has_mpc_fcw:
+      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      return
+
+    # Standstill: use blended
+    if self._standstill_count > 3:
+      self._mode_manager.request_mode('blended', confidence=0.9)
+      return
+
+    # Slow down scenarios: emergency for high urgency, normal for lower urgency
+    if self._has_slow_down:
+      if self._urgency > 0.7:
+        # Emergency: immediate blended mode for high urgency stops
+        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      else:
+        # Normal: blended with urgency-based confidence
+        confidence = min(1.0, self._urgency * 1.5)
+        self._mode_manager.request_mode('blended', confidence=confidence)
+      return
+
+    # Driving slow: use ACC (but not if actively slowing down)
+    if self._has_slowness and not self._has_slow_down:
+      self._mode_manager.request_mode('acc', confidence=0.8)
+      return
+
+    # Default: ACC
+    self._mode_manager.request_mode('acc', confidence=0.7)
+
+  def _radar_mode(self) -> None:
+    """Radar mode with emergency handling."""
+
+    # EMERGENCY: MPC FCW - immediate blended mode
+    if self._has_mpc_fcw:
+      self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      return
+
+    # If lead detected and not in standstill: always use ACC
+    if self._has_lead_filtered and not (self._standstill_count > 3):
+      self._mode_manager.request_mode('acc', confidence=1.0)
+      return
+
+    # Slow down scenarios: emergency for high urgency, normal for lower urgency
+    if self._has_slow_down:
+      if self._urgency > 0.7:
+        # Emergency: immediate blended mode for high urgency stops
+        self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
+      else:
+        # Normal: blended with urgency-based confidence
+        confidence = min(1.0, self._urgency * 1.3)
+        self._mode_manager.request_mode('blended', confidence=confidence)
+      return
+
+    # Standstill: use blended
+    if self._standstill_count > 3:
+      self._mode_manager.request_mode('blended', confidence=0.9)
+      return
+
+    # Driving slow: use ACC (but not if actively slowing down)
+    if self._has_slowness and not self._has_slow_down:
+      self._mode_manager.request_mode('acc', confidence=0.8)
+      return
+
+    # Default: ACC
+    self._mode_manager.request_mode('acc', confidence=0.7)
+
+  def update(self, sm: messaging.SubMaster) -> None:
+    self._read_params()
+
+    self.set_mpc_fcw_crash_cnt()
+
+    self._update_calculations(sm)
+
+    if self._CP.radarUnavailable:
+      self._radarless_mode()
+    else:
+      self._radar_mode()
+
+    self._mode_manager.update()
+    self._active = sm['selfdriveState'].experimentalMode and self._enabled
+    self._frame += 1

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

@@ -0,0 +1,94 @@
+import pytest
+
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+
+class MockLeadOne:
+  def __init__(self, status=0.0):
+    self.status = status
+
+class MockRadarState:
+  def __init__(self, status=0.0):
+    self.leadOne = MockLeadOne(status=status)
+
+class MockCarState:
+  def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
+    self.vEgo = vEgo
+    self.vCruise = vCruise
+    self.standstill = standstill
+
+class MockModelData:
+  def __init__(self, valid=True):
+    size = 33 if valid else 10  # incomplete if invalid
+    self.position = type("Pos", (), {"x": [0.0] * size})()
+    self.orientation = type("Ori", (), {"x": [0.0] * size})()
+
+class MockSelfDriveState:
+  def __init__(self, experimentalMode=False):
+    self.experimentalMode = experimentalMode
+
+class MockParams:
+  def get_bool(self, name):
+    return True
+
+@pytest.fixture
+def default_sm():
+  sm = {
+    'carState': MockCarState(vEgo=10.0, vCruise=20.0),
+    'radarState': MockRadarState(status=1.0),
+    'modelV2': MockModelData(valid=True),
+    'selfdriveState': MockSelfDriveState(experimentalMode=True),
+  }
+  return sm
+
+@pytest.fixture
+def mock_cp():
+  class CP:
+    radarUnavailable = False
+  return CP()
+
+@pytest.fixture
+def mock_mpc():
+  class MPC:
+    crash_cnt = 0
+  return MPC()
+
+# Fake Kalman Filter that always returns a given value
+class FakeKalman:
+  def __init__(self, value=1.0):
+    self.value = value
+  def add_data(self, v): pass
+  def get_value(self): return self.value
+  def get_confidence(self): return 1.0
+  def reset_data(self): pass
+
+def test_initial_mode_is_acc(mock_cp, mock_mpc):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  assert controller.mode() == "acc"
+
+def test_standstill_triggers_blended(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['carState'].standstill = True
+  for _ in range(10):
+    controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+def test_emergency_blended_on_fcw(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  mock_mpc.crash_cnt = 1  # simulate FCW
+  for _ in range(2):
+    controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+def test_radarless_slowdown_triggers_blended(mock_cp, mock_mpc, default_sm):
+  mock_cp.radarUnavailable = True
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+
+  # Force conditions to simulate slowdown
+  controller._slow_down_filter = FakeKalman(value=1.0)  # Ensure urgency triggers slowdown
+  controller._v_ego_kph = 35.0
+  default_sm['modelV2'] = MockModelData(valid=False)  # Incomplete trajectory
+
+  for _ in range(3):
+    controller.update(default_sm)
+
+  assert controller.mode() == "blended"

openpilot/sunnypilot/selfdrive/controls/lib/e2e_alerts_helper.py

@@ -0,0 +1,170 @@
+"""
+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.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
+
+GREEN_LIGHT_X_THRESHOLD = 30
+LEAD_DEPART_DIST_THRESHOLD = 1.0
+TRIGGER_TIMER_THRESHOLD = 0.3
+
+
+class E2EStates:
+  INACTIVE = 0
+  ARMED = 1
+  CONSUMED = 2
+
+
+class E2EAlertsHelper:
+  def __init__(self):
+    self._params = Params()
+    self.frame = -1
+    self.green_light_state = E2EStates.INACTIVE
+    self.prev_green_light_state = E2EStates.INACTIVE
+    self.lead_depart_state = E2EStates.INACTIVE
+    self.prev_lead_depart_state = E2EStates.INACTIVE
+
+    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")
+
+    self.green_light_trigger_timer = 0
+    self.lead_depart_trigger_timer = 0
+    self.last_lead_distance = -1
+    self.last_moving_frame = -1
+
+    self.allowed = False
+    self.last_allowed = False
+    self.has_lead = False
+
+    self.lead_depart_arm_timer = 0
+    self.lead_depart_confirmed_lead = False
+    self.lead_depart_armed = False
+
+  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")
+
+  def update_alert_trigger(self, sm: messaging.SubMaster):
+    CS = sm['carState']
+    CC = sm['carControl']
+
+    model_x = sm['modelV2'].position.x
+    max_idx = len(model_x) - 1
+    self.has_lead = sm['radarState'].leadOne.present
+    lead_dRel = sm['radarState'].leadOne.dRel
+
+    standstill = CS.standstill
+    moving = not standstill and CS.vEgo > 0.1
+
+    if moving:
+      self.last_moving_frame = self.frame
+    recent_moving = self.last_moving_frame == -1 or (self.frame - self.last_moving_frame) * DT_MDL < 2.0
+
+    self.allowed = not moving and not CS.gasPressed and not CC.enabled and not recent_moving
+
+    # Green Light Alert
+    green_light_trigger = False
+    if self.green_light_state == E2EStates.ARMED:
+      if model_x[max_idx] > GREEN_LIGHT_X_THRESHOLD:
+        self.green_light_trigger_timer += 1
+      else:
+        self.green_light_trigger_timer = 0
+
+      if self.green_light_trigger_timer * DT_MDL > TRIGGER_TIMER_THRESHOLD:
+        green_light_trigger = True
+    elif self.green_light_state != E2EStates.ARMED:
+      self.green_light_trigger_timer = 0
+
+    # Lead Departure Alert
+    close_lead_valid = self.has_lead and lead_dRel < 8.0
+    if self.allowed and not self.last_allowed and close_lead_valid:
+      self.lead_depart_confirmed_lead = True
+    elif not self.allowed:
+      self.lead_depart_confirmed_lead = False
+
+    if self.allowed and self.lead_depart_confirmed_lead and close_lead_valid:
+      self.lead_depart_arm_timer += 1
+
+      if self.lead_depart_arm_timer * DT_MDL >= 1.0:
+        self.lead_depart_armed = True
+    else:
+      self.lead_depart_arm_timer = 0
+      self.lead_depart_armed = False
+
+    lead_depart_trigger = False
+    if self.lead_depart_state == E2EStates.ARMED:
+      if self.last_lead_distance == -1 or lead_dRel < self.last_lead_distance:
+        self.last_lead_distance = lead_dRel
+
+      if self.last_lead_distance != -1 and (lead_dRel - self.last_lead_distance > LEAD_DEPART_DIST_THRESHOLD):
+        self.lead_depart_trigger_timer += 1
+      else:
+        self.lead_depart_trigger_timer = 0
+
+      if self.lead_depart_trigger_timer * DT_MDL > TRIGGER_TIMER_THRESHOLD:
+        lead_depart_trigger = True
+    elif self.lead_depart_state != E2EStates.ARMED:
+      self.last_lead_distance = -1
+      self.lead_depart_trigger_timer = 0
+
+    self.last_allowed = self.allowed
+
+    return green_light_trigger, lead_depart_trigger
+
+  @staticmethod
+  def update_state_machine(state: int, enabled: bool, allowed: bool, triggered: bool) -> tuple[int, bool]:
+    if state != E2EStates.INACTIVE:
+      if not allowed or not enabled:
+        state = E2EStates.INACTIVE
+
+      else:
+        if state == E2EStates.ARMED:
+          if triggered:
+            state = E2EStates.CONSUMED
+
+        elif state == E2EStates.CONSUMED:
+          pass
+
+    elif state == E2EStates.INACTIVE:
+      if allowed and enabled:
+        state = E2EStates.ARMED
+
+    return state, triggered
+
+  def update(self, sm: messaging.SubMaster, events_sp: EventsSP) -> None:
+    self._read_params()
+
+    green_light_trigger, lead_depart_trigger = self.update_alert_trigger(sm)
+
+    self.prev_green_light_state = self.green_light_state
+    self.prev_lead_depart_state = self.lead_depart_state
+
+    self.green_light_state, self.green_light_alert = self.update_state_machine(
+      self.green_light_state,
+      self.green_light_alert_enabled,
+      self.allowed and not self.has_lead,
+      green_light_trigger
+    )
+
+    self.lead_depart_state, self.lead_depart_alert = self.update_state_machine(
+      self.lead_depart_state,
+      self.lead_depart_alert_enabled,
+      self.allowed and self.lead_depart_armed,
+      lead_depart_trigger
+    )
+
+    if self.green_light_alert or self.lead_depart_alert:
+      events_sp.add(custom.OnroadEventSP.EventName.e2eChime)
+
+    self.frame += 1

openpilot/sunnypilot/selfdrive/controls/lib/lane_turn_desire.py

@@ -0,0 +1,47 @@
+"""
+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.cereal import custom
+
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+
+TurnDirection = custom.ModelDataV2SP.TurnDirection
+
+LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
+
+
+class LaneTurnController:
+  def __init__(self, desire_helper):
+    self.DH = desire_helper
+    self.turn_direction = TurnDirection.none
+    self.params = Params()
+    self.lane_turn_value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS
+    self.param_read_counter = 0
+    self.enabled = self.params.get_bool("LaneTurnDesire")
+
+  def read_params(self):
+    self.enabled = self.params.get_bool("LaneTurnDesire")
+    value = float(self.params.get("LaneTurnValue", return_default=True)) * CV.MPH_TO_MS
+    self.lane_turn_value = min(float(LANE_CHANGE_SPEED_MIN), value)
+
+  def update_params(self) -> None:
+    if self.param_read_counter % 50 == 0:
+      self.read_params()
+    self.param_read_counter += 1
+
+  def update_lane_turn(self, blindspot_left: bool, blindspot_right: bool, left_blinker: bool, right_blinker: bool, v_ego: float) -> None:
+    if left_blinker and not right_blinker and v_ego < self.lane_turn_value and not blindspot_left:
+      self.turn_direction = TurnDirection.turnLeft
+    elif right_blinker and not left_blinker and v_ego < self.lane_turn_value and not blindspot_right:
+      self.turn_direction = TurnDirection.turnRight
+    else:
+      self.turn_direction = TurnDirection.none
+
+  def get_turn_direction(self):
+    if not self.enabled:
+      return TurnDirection.none
+    return self.turn_direction

openpilot/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py

@@ -0,0 +1,38 @@
+"""
+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.sunnypilot.selfdrive.controls.lib.nnlc.nnlc import NeuralNetworkLateralControl
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_override import LatControlTorqueExtOverride
+
+
+class LatControlTorqueExt(NeuralNetworkLateralControl, LatControlTorqueExtOverride):
+  def __init__(self, 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,
+             desired_curvature, actual_curvature, steer_limited_by_safety, output_torque):
+    self._ff = ff
+    self._pid = pid
+    self._pid_log = pid_log
+    self._setpoint = setpoint
+    self._measurement = measurement
+    self._roll_compensation = roll_compensation
+    self._lateral_accel_deadzone = lateral_accel_deadzone
+    self._desired_lateral_accel = desired_lateral_accel
+    self._actual_lateral_accel = actual_lateral_accel
+    self._desired_curvature = desired_curvature
+    self._actual_curvature = actual_curvature
+    self._gravity_adjusted_lateral_accel = gravity_adjusted_lateral_accel
+    self._steer_limited_by_safety = steer_limited_by_safety
+    self._output_torque = output_torque
+
+    self.update_calculations(CS, VM, desired_lateral_accel)
+    self.update_neural_network_feedforward(CS, params, calibrated_pose)
+
+    return self._pid_log, self._output_torque

openpilot/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py

@@ -0,0 +1,134 @@
+"""
+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 math
+import numpy as np
+
+from openpilot.common.pid import PIDController
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from openpilot.selfdrive.modeld.constants import ModelConstants
+
+LAT_PLAN_MIN_IDX = 5
+LATERAL_LAG_MOD = 0.0  # seconds, modifies how far in the future we look ahead for the lateral plan
+
+KP = 1.0
+KI = 0.3
+
+
+def get_predicted_lateral_jerk(lat_accels, t_diffs):
+  # compute finite difference between subsequent model_v2.acceleration.y values
+  # this is just two calls of np.diff followed by an element-wise division
+  lat_accel_diffs = np.diff(lat_accels)
+  lat_jerk = lat_accel_diffs / t_diffs
+  # return as python list
+  return lat_jerk.tolist()
+
+
+def sign(x):
+  return 1.0 if x > 0.0 else (-1.0 if x < 0.0 else 0.0)
+
+
+def get_lookahead_value(future_vals, current_val):
+  if len(future_vals) == 0:
+    return current_val
+
+  same_sign_vals = [v for v in future_vals if sign(v) == sign(current_val)]
+
+  # if any future val has opposite sign of current val, return 0
+  if len(same_sign_vals) < len(future_vals):
+    return 0.0
+
+  # otherwise return the value with minimum absolute value
+  min_val = min(same_sign_vals + [current_val], key=lambda x: abs(x))
+  return min_val
+
+
+class LatControlTorqueExtBase:
+  def __init__(self, lac_torque, CP, CP_SP, CI):
+    self.model_v2 = None
+    self.model_valid = False
+    self.lac_torque = lac_torque
+
+    self.actual_lateral_jerk: float = 0.0
+    self.lateral_jerk_setpoint: float = 0.0
+    self.lateral_jerk_measurement: float = 0.0
+    self.lookahead_lateral_jerk: float = 0.0
+
+    self.torque_from_lateral_accel_in_torque_space = CI.torque_from_lateral_accel_in_torque_space()
+    self.torque_params = lac_torque.torque_params
+
+    self._ff = 0.0
+    self._pid = PIDController(KP, KI)
+    self._pid_log = None
+    self._setpoint = 0.0
+    self._measurement = 0.0
+    self._roll_compensation = 0.0
+    self._lateral_accel_deadzone = 0.0
+    self._desired_lateral_accel = 0.0
+    self._actual_lateral_accel = 0.0
+    self._desired_curvature = 0.0
+    self._actual_curvature = 0.0
+    self._gravity_adjusted_lateral_accel = 0.0
+    self._steer_limited_by_safety = False
+    self._output_torque = 0.0
+
+    # twilsonco's Lateral Neural Network Feedforward
+    # Instantaneous lateral jerk changes very rapidly, making it not useful on its own,
+    # however, we can "look ahead" to the future planned lateral jerk in order to gauge
+    # whether the current desired lateral jerk will persist into the future, i.e.
+    # whether it's "deliberate" or not. This allows us to simply ignore short-lived jerk.
+    # Note that LAT_PLAN_MIN_IDX is defined above and is used in order to prevent
+    # using a "future" value that is actually planned to occur before the "current" desired
+    # value, which is offset by the steerActuatorDelay.
+    # TODO-SP: Reevaluate lookahead v values that determines how low a desired lateral jerk signal needs to
+    #          persist in order to be used.
+    self.friction_look_ahead_v = [1.4, 2.0]  # how many seconds in the future to look ahead in [0, ~2.1] in 0.1 increments
+    self.friction_look_ahead_bp = [9.0, 30.0]  # corresponding speeds in m/s in [0, ~40] in 1.0 increments
+
+    # Scaling the lateral acceleration "friction response" could be helpful for some.
+    # Increase for a stronger response, decrease for a weaker response.
+    self.lat_jerk_friction_factor = 0.4
+    self.lat_accel_friction_factor = 0.7  # in [0, 3], in 0.05 increments. 3 is arbitrary safety limit
+
+    # precompute time differences between ModelConstants.T_IDXS
+    self.t_diffs = np.diff(ModelConstants.T_IDXS)
+    self.desired_lat_jerk_time = CP.steerActuatorDelay + LATERAL_LAG_MOD
+
+  def update_model_v2(self, model_v2):
+    self.model_v2 = model_v2
+    self.model_valid = self.model_v2 is not None and len(self.model_v2.orientation.x) >= CONTROL_N
+
+  def update_lateral_lag(self, lag):
+    self.desired_lat_jerk_time = max(0.01, lag) + LATERAL_LAG_MOD
+
+  def update_friction_input(self, val_1, val_2):
+    _error = val_1 - val_2
+    _value = self.lat_accel_friction_factor * _error + self.lat_jerk_friction_factor * self.lookahead_lateral_jerk
+
+    return _value
+
+  def update_calculations(self, CS, VM, desired_lateral_accel):
+    self.actual_lateral_jerk = 0.0
+    self.lateral_jerk_setpoint = 0.0
+    self.lateral_jerk_measurement = 0.0
+    self.lookahead_lateral_jerk = 0.0
+
+    actual_curvature_rate = -VM.calc_curvature(math.radians(CS.steeringRateDeg), CS.vEgo, 0.0)
+    self.actual_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2
+
+    if self.model_valid:
+      # prepare "look-ahead" desired lateral jerk
+      lookahead = np.interp(CS.vEgo, self.friction_look_ahead_bp, self.friction_look_ahead_v)
+      friction_upper_idx = next((i for i, val in enumerate(ModelConstants.T_IDXS) if val > lookahead), 16)
+      predicted_lateral_jerk = get_predicted_lateral_jerk(self.model_v2.acceleration.y, self.t_diffs)
+      desired_lateral_jerk = (np.interp(self.desired_lat_jerk_time, ModelConstants.T_IDXS,
+                              self.model_v2.acceleration.y) - desired_lateral_accel) / self.desired_lat_jerk_time
+      self.lookahead_lateral_jerk = get_lookahead_value(predicted_lateral_jerk[LAT_PLAN_MIN_IDX:friction_upper_idx], desired_lateral_jerk)
+      if self.lookahead_lateral_jerk == 0.0:
+        self.actual_lateral_jerk = 0.0
+        self.lat_accel_friction_factor = 1.0
+      self.lateral_jerk_setpoint = self.lat_jerk_friction_factor * self.lookahead_lateral_jerk
+      self.lateral_jerk_measurement = self.lat_jerk_friction_factor * self.actual_lateral_jerk

openpilot/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

openpilot/sunnypilot/selfdrive/controls/lib/latcontrol_torque_v0.py

@@ -0,0 +1,128 @@
+import math
+import numpy as np
+from collections import deque
+
+from openpilot.cereal import log
+from opendbc.car.lateral import get_friction
+from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.common.pid import PIDController
+
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext import LatControlTorqueExt
+
+# At higher speeds (25+mph) we can assume:
+# Lateral acceleration achieved by a specific car correlates to
+# torque applied to the steering rack. It does not correlate to
+# wheel slip, or to speed.
+
+# This controller applies torque to achieve desired lateral
+# accelerations. To compensate for the low speed effects the
+# proportional gain is increased at low speeds by the PID controller.
+# Additionally, there is friction in the steering wheel that needs
+# to be overcome to move it at all, this is compensated for too.
+
+KP = 1.0
+KI = 0.3
+KD = 0.0
+INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30]
+KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP]
+
+LP_FILTER_CUTOFF_HZ = 1.2
+LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
+FRICTION_THRESHOLD = 0.3
+VERSION = 0
+
+
+class LatControlTorque(LatControl):
+  def __init__(self, CP, CP_SP, CI, dt):
+    super().__init__(CP, CP_SP, CI, dt)
+    self.torque_params = CP.lateralTuning.torque.as_builder()
+    self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
+    self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
+    self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, KD, rate=1/self.dt)
+    self.update_limits()
+    self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
+    self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
+    self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len)
+    self.previous_measurement = 0.0
+    self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
+
+    self.extension = LatControlTorqueExt(self, CP, CP_SP, CI)
+
+  def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
+    self.torque_params.latAccelFactor = latAccelFactor
+    self.torque_params.latAccelOffset = latAccelOffset
+    self.torque_params.friction = friction
+    self.update_limits()
+
+  def update_limits(self):
+    self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params),
+                        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, lat_delay):
+    # 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()
+    pid_log.version = VERSION
+    if not active:
+      output_torque = 0.0
+      pid_log.active = False
+    else:
+      measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+      roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY
+      curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
+      lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
+
+      delay_frames = int(np.clip(lat_delay / self.dt, 1, self.lat_accel_request_buffer_len))
+      expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames]
+      # TODO factor out lateral jerk from error to later replace it with delay independent alternative
+      future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
+      self.lat_accel_request_buffer.append(future_desired_lateral_accel)
+      gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
+      desired_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / lat_delay
+
+      measurement = measured_curvature * CS.vEgo ** 2
+      measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt)
+      self.previous_measurement = measurement
+
+      setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel
+      error = setpoint - measurement
+
+      # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly
+      pid_log.error = float(error)
+      ff = gravity_adjusted_future_lateral_accel
+      # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll
+      ff -= self.torque_params.latAccelOffset
+      # TODO jerk is weighted by lat_delay for legacy reasons, but should be made independent of it
+      ff += get_friction(error, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
+
+      freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
+      output_lataccel = self.pid.update(pid_log.error,
+                                       -measurement_rate,
+                                        feedforward=ff,
+                                        speed=CS.vEgo,
+                                        freeze_integrator=freeze_integrator)
+      output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
+
+      # Lateral acceleration torque controller extension updates
+      # Overrides pid_log.error and output_torque
+      pid_log, output_torque = self.extension.update(CS, VM, self.pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation,
+                                                     future_desired_lateral_accel, measurement, lateral_accel_deadzone, gravity_adjusted_future_lateral_accel,
+                                                     desired_curvature, measured_curvature, steer_limited_by_safety, output_torque)
+
+      pid_log.active = True
+      pid_log.p = float(self.pid.p)
+      pid_log.i = float(self.pid.i)
+      pid_log.d = float(self.pid.d)
+      pid_log.f = float(self.pid.f)
+      pid_log.output = float(-output_torque) # TODO: log lat accel?
+      pid_log.actualLateralAccel = float(measurement)
+      pid_log.desiredLateralAccel = float(setpoint)
+      pid_log.desiredLateralJerk = float(desired_lateral_jerk)
+      pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
+
+    # TODO left is positive in this convention
+    return -output_torque, 0.0, pid_log

openpilot/sunnypilot/selfdrive/controls/lib/latcontrol_torque_versions.json

@@ -0,0 +1,8 @@
+{
+  "v1.0": {
+    "version": "1.0"
+  },
+  "v0.0": {
+    "version": "0.0"
+  }
+}

openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -0,0 +1,141 @@
+"""
+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.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
+LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
+
+
+class LongitudinalPlannerSP:
+  def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
+    self.events_sp = EventsSP()
+    self.resolver = SpeedLimitResolver()
+    self.dec = DynamicExperimentalController(CP, mpc)
+    self.scc = SmartCruiseControl()
+    self.resolver = SpeedLimitResolver()
+    self.sla = SpeedLimitAssist(CP, CP_SP)
+    self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
+    self.source = LongitudinalPlanSource.cruise
+    self.e2e_alerts_helper = E2EAlertsHelper()
+
+    self.output_v_target = 0.
+    self.output_a_target = 0.
+
+  def is_e2e(self, sm: messaging.SubMaster) -> bool:
+    experimental_mode = sm['selfdriveState'].experimentalMode
+    if not self.dec.active():
+      return experimental_mode
+
+    return experimental_mode and self.dec.mode() == "blended"
+
+  def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
+    CS = sm['carState']
+    v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
+    v_cruise_cluster = v_cruise_cluster_kph * CV.KPH_TO_MS
+
+    long_enabled = sm['carControl'].enabled
+    long_override = sm['carControl'].cruiseControl.override
+
+    # Smart Cruise Control
+    self.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 = {
+      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])
+    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')
+
+    plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
+
+    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
+    dec.state = DecState.blended if self.dec.mode() == 'blended' else DecState.acc
+    dec.enabled = self.dec.enabled()
+    dec.active = self.dec.active()
+
+    # Smart Cruise Control
+    smartCruiseControl = longitudinalPlanSP.smartCruiseControl
+    # Vision Control
+    sccVision = smartCruiseControl.vision
+    sccVision.state = self.scc.vision.state
+    sccVision.vTarget = float(self.scc.vision.output_v_target)
+    sccVision.aTarget = float(self.scc.vision.output_a_target)
+    sccVision.currentLateralAccel = float(self.scc.vision.current_lat_acc)
+    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)

openpilot/sunnypilot/selfdrive/controls/lib/nnlc/helpers.py

@@ -0,0 +1,68 @@
+"""
+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 os
+import tomllib
+from difflib import SequenceMatcher
+
+from opendbc.car import structs
+from openpilot.common.basedir import BASEDIR
+
+TORQUE_NN_MODEL_PATH = os.path.join(BASEDIR, "openpilot", "sunnypilot", "neural_network_data", "neural_network_lateral_control")
+TORQUE_NN_MODEL_SUBSTITUTE_PATH = os.path.join(BASEDIR, "opendbc_repo", "opendbc", "car", "torque_data/substitute.toml")
+MOCK_MODEL_PATH = os.path.join(TORQUE_NN_MODEL_PATH, "MOCK.json")
+
+
+def similarity(s1: str, s2: str) -> float:
+  return SequenceMatcher(None, s1, s2).ratio()
+
+
+def get_nn_model_path(CP: structs.CarParams) -> tuple[str, str, bool]:
+  car_fingerprint = CP.carFingerprint
+  eps_fw = str(next((fw.fwVersion for fw in CP.carFw if fw.ecu == "eps"), ""))
+
+  def check_nn_path(_nn_candidate):
+    _model_path = None
+    _max_similarity = -1.0
+    for f in os.listdir(TORQUE_NN_MODEL_PATH):
+      if f.endswith(".json"):
+        model = os.path.splitext(f)[0]
+        similarity_score = similarity(model, _nn_candidate)
+        if similarity_score > _max_similarity:
+          _max_similarity = similarity_score
+          _model_path = os.path.join(TORQUE_NN_MODEL_PATH, f)
+    return _model_path, _max_similarity
+
+  if len(eps_fw) > 3:
+    eps_fw = eps_fw.replace("\\", "")
+    nn_candidate = f"{car_fingerprint} {eps_fw}"
+  else:
+    nn_candidate = car_fingerprint
+
+  model_path, max_similarity = check_nn_path(nn_candidate)
+  exact_match = max_similarity >= 0.99
+
+  if car_fingerprint not in model_path or 0.0 <= max_similarity < 0.9:
+    nn_candidate = car_fingerprint
+    model_path, max_similarity = check_nn_path(nn_candidate)
+    exact_match = max_similarity >= 0.99
+
+    if 0.0 <= max_similarity < 0.9:
+      with open(TORQUE_NN_MODEL_SUBSTITUTE_PATH, 'rb') as f:
+        sub = tomllib.load(f)
+      sub_candidate = sub.get(car_fingerprint, car_fingerprint)
+
+      for candidate in [car_fingerprint, sub_candidate]:
+        model_path, max_similarity = check_nn_path(candidate)
+
+      exact_match = False
+
+  if CP.steerControlType == structs.CarParams.SteerControlType.angle:
+    model_path = MOCK_MODEL_PATH
+
+  model_name = os.path.splitext(os.path.basename(model_path))[0]
+
+  return model_path, model_name, exact_match

openpilot/sunnypilot/selfdrive/controls/lib/nnlc/model.py

@@ -0,0 +1,83 @@
+"""
+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 json import load
+import numpy as np
+
+from openpilot.selfdrive.modeld.parse_model_outputs import safe_exp
+
+# dict used to rename activation functions whose names aren't valid python identifiers
+ACTIVATION_FUNCTION_NAMES = {'σ': 'sigmoid'}
+
+
+class NNTorqueModel:
+  def __init__(self, params_file, zero_bias=False):
+    with open(params_file) as f:
+      params = load(f)
+
+    self.input_size = params["input_size"]
+    self.output_size = params["output_size"]
+    self.input_mean = np.array(params["input_mean"], dtype=np.float32).T
+    self.input_std = np.array(params["input_std"], dtype=np.float32).T
+    self.layers = []
+    self.friction_override = False
+
+    for layer_params in params["layers"]:
+      W = np.array(layer_params[next(key for key in layer_params.keys() if key.endswith('_W'))], dtype=np.float32).T
+      b = np.array(layer_params[next(key for key in layer_params.keys() if key.endswith('_b'))], dtype=np.float32).T
+      if zero_bias:
+        b = np.zeros_like(b)
+      activation = layer_params["activation"]
+      for k, v in ACTIVATION_FUNCTION_NAMES.items():
+        activation = activation.replace(k, v)
+      self.layers.append((W, b, activation))
+
+    self.validate_layers()
+    self.check_for_friction_override()
+
+  # Begin activation functions.
+  # These are called by name using the keys in the model json file
+  @staticmethod
+  def sigmoid(x):
+    return 1 / (1 + safe_exp(-x))
+
+  @staticmethod
+  def identity(x):
+    return x
+  # End activation functions
+
+  def forward(self, x):
+    for W, b, activation in self.layers:
+      x = getattr(self, activation)(x.dot(W) + b)
+    return x
+
+  def evaluate(self, input_array):
+    in_len = len(input_array)
+    if in_len != self.input_size:
+      # If the input is length 2-4, then it's a simplified evaluation.
+      # In that case, need to add on zeros to fill out the input array to match the correct length.
+      if 2 <= in_len:
+        input_array = input_array + [0] * (self.input_size - in_len)
+      else:
+        raise ValueError(f"Input array length {len(input_array)} must be length 2 or greater")
+
+    input_array = np.array(input_array, dtype=np.float32)
+
+    # Rescale the input array using the input_mean and input_std
+    input_array = (input_array - self.input_mean) / self.input_std
+
+    output_array = self.forward(input_array)
+
+    return float(output_array[0, 0])
+
+  def validate_layers(self):
+    for _, _, activation in self.layers:
+      if not hasattr(self, activation):
+        raise ValueError(f"Unknown activation: {activation}")
+
+  def check_for_friction_override(self):
+    y = self.evaluate([10.0, 0.0, 0.2])
+    self.friction_override = (y < 0.1)

openpilot/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py

@@ -0,0 +1,164 @@
+"""
+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 collections import deque
+import math
+import numpy as np
+
+from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction
+from opendbc.sunnypilot.car.interfaces import LatControlInputs
+from opendbc.sunnypilot.car.lateral_ext import get_friction as get_friction_in_torque_space
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.params import Params
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_base import LatControlTorqueExtBase, sign
+from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import MOCK_MODEL_PATH
+from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.model import NNTorqueModel
+
+LOW_SPEED_X = [0, 10, 20, 30]
+LOW_SPEED_Y = [12, 3, 1, 0]
+
+
+# At a given roll, if pitch magnitude increases, the
+# gravitational acceleration component starts pointing
+# in the longitudinal direction, decreasing the lateral
+# acceleration component. Here we do the same thing
+# to the roll value itself, then passed to nnff.
+def roll_pitch_adjust(roll, pitch):
+  return roll * math.cos(pitch)
+
+
+class NeuralNetworkLateralControl(LatControlTorqueExtBase):
+  def __init__(self, lac_torque, CP, CP_SP, CI):
+    super().__init__(lac_torque, CP, CP_SP, CI)
+    self.params = Params()
+    self.enabled = self.params.get_bool("NeuralNetworkLateralControl")
+    self.has_nn_model = CP_SP.neuralNetworkLateralControl.model.path != MOCK_MODEL_PATH
+
+    # NN model takes current v_ego, lateral_accel, lat accel/jerk error, roll, and past/future/planned data
+    # of lat accel and roll
+    # Past value is computed using previous desired lat accel and observed roll
+    self.model = NNTorqueModel(CP_SP.neuralNetworkLateralControl.model.path)
+
+    self.pitch = FirstOrderFilter(0.0, 0.5, 0.01)
+    self.pitch_last = 0.0
+
+    # setup future time offsets
+    self.future_times = [0.3, 0.6, 1.0, 1.5] # seconds in the future
+    self.nn_future_times = [i + self.desired_lat_jerk_time for i in self.future_times]
+
+    # setup past time offsets
+    self.past_times = [-0.3, -0.2, -0.1]
+    history_check_frames = [int(abs(i)*100) for i in self.past_times]
+    self.history_frame_offsets = [history_check_frames[0] - i for i in history_check_frames]
+    self.lateral_accel_desired_deque = deque(maxlen=history_check_frames[0])
+    self.roll_deque = deque(maxlen=history_check_frames[0])
+    self.error_deque = deque(maxlen=history_check_frames[0])
+    self.past_future_len = len(self.past_times) + len(self.nn_future_times)
+
+  @property
+  def _nnlc_enabled(self):
+    return self.enabled and self.model_valid and self.has_nn_model
+
+  def update_limits(self):
+    if not self._nnlc_enabled:
+      return
+
+    self._pid.set_limits(self.lac_torque.steer_max, -self.lac_torque.steer_max)
+
+  def update_lateral_lag(self, lag):
+    super().update_lateral_lag(lag)
+    self.nn_future_times = [t + self.desired_lat_jerk_time for t in self.future_times]
+
+  def update_feedforward_torque_space(self, CS):
+    torque_from_setpoint = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._setpoint, self._roll_compensation, CS.vEgo, CS.aEgo),
+                                                                          self.torque_params, gravity_adjusted=False)
+    torque_from_measurement = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._measurement, self._roll_compensation, CS.vEgo, CS.aEgo),
+                                                                             self.torque_params, gravity_adjusted=False)
+    self._pid_log.error = float(torque_from_setpoint - torque_from_measurement)
+    self._ff = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._gravity_adjusted_lateral_accel, self._roll_compensation,
+                                                                               CS.vEgo, CS.aEgo), self.torque_params, gravity_adjusted=True)
+    self._ff += get_friction_in_torque_space(self._desired_lateral_accel - self._actual_lateral_accel, self._lateral_accel_deadzone,
+                                             FRICTION_THRESHOLD, self.torque_params)
+
+  def update_output_torque(self, CS):
+    freeze_integrator = self._steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
+    self._output_torque = self._pid.update(self._pid_log.error,
+                                           feedforward=self._ff,
+                                           speed=CS.vEgo,
+                                           freeze_integrator=freeze_integrator)
+
+  def update_neural_network_feedforward(self, CS, params, calibrated_pose) -> None:
+    if not self._nnlc_enabled:
+      return
+
+    self.update_feedforward_torque_space(CS)
+
+    low_speed_factor = float(np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)) ** 2
+    self._setpoint = self._desired_lateral_accel + low_speed_factor * self._desired_curvature
+    self._measurement = self._actual_lateral_accel + low_speed_factor * self._actual_curvature
+
+    # update past data
+    roll = params.roll
+    if calibrated_pose is not None:
+      pitch = self.pitch.update(calibrated_pose.orientation.pitch)
+      roll = roll_pitch_adjust(roll, pitch)
+      self.pitch_last = pitch
+    self.roll_deque.append(roll)
+    self.lateral_accel_desired_deque.append(self._desired_lateral_accel)
+
+    # prepare past and future values
+    # adjust future times to account for longitudinal acceleration
+    adjusted_future_times = [t + 0.5 * CS.aEgo * (t / max(CS.vEgo, 1.0)) for t in self.nn_future_times]
+    past_rolls = [self.roll_deque[min(len(self.roll_deque) - 1, i)] for i in self.history_frame_offsets]
+    future_rolls = [roll_pitch_adjust(np.interp(t, ModelConstants.T_IDXS, self.model_v2.orientation.x) + roll,
+                                      np.interp(t, ModelConstants.T_IDXS, self.model_v2.orientation.y) + self.pitch_last) for t in
+                    adjusted_future_times]
+    past_lateral_accels_desired = [self.lateral_accel_desired_deque[min(len(self.lateral_accel_desired_deque) - 1, i)]
+                                   for i in self.history_frame_offsets]
+    future_planned_lateral_accels = [np.interp(t, ModelConstants.T_IDXS, self.model_v2.acceleration.y) for t in
+                                     adjusted_future_times]
+
+    # compute NNFF error response
+    nnff_setpoint_input = [CS.vEgo, self._setpoint, self.lateral_jerk_setpoint, roll] \
+                          + [self._setpoint] * self.past_future_len \
+                          + past_rolls + future_rolls
+    # past lateral accel error shouldn't count, so use past desired like the setpoint input
+    nnff_measurement_input = [CS.vEgo, self._measurement, self.lateral_jerk_measurement, roll] \
+                             + [self._measurement] * self.past_future_len \
+                             + past_rolls + future_rolls
+    torque_from_setpoint = self.model.evaluate(nnff_setpoint_input)
+    torque_from_measurement = self.model.evaluate(nnff_measurement_input)
+    self._pid_log.error = torque_from_setpoint - torque_from_measurement
+
+    # The "pure" NNLC error response can be too weak for cars whose models were trained
+    # with a lack of high-magnitude lateral acceleration data, for which the NNLC model
+    # torque response flattens out at high lateral accelerations.
+    # This workaround blends in a guaranteed stronger error response only when the
+    # desired lateral acceleration is high enough to warrant it, by using the lateral acceleration
+    # error as the input to the NNLC model. This is not ideal, and potentially degrades the NNLC
+    # accuracy for cars that don't have this issue, but it's necessary until a better NNLC model
+    # structure is used that doesn't create this issue when high-magnitude data is missing.
+    error_blend_factor = float(np.interp(abs(self._desired_lateral_accel), [1.0, 2.0], [0.0, 1.0]))
+    if error_blend_factor > 0.0:  # blend in stronger error response when in high lat accel
+      # NNFF inputs 5+ are optional, and if left out are replaced with 0.0 inside the NNFF class
+      nnff_error_input = [CS.vEgo, self._setpoint - self._measurement, self.lateral_jerk_setpoint - self.lateral_jerk_measurement, 0.0]
+      torque_from_error = self.model.evaluate(nnff_error_input)
+      if sign(self._pid_log.error) == sign(torque_from_error) and abs(self._pid_log.error) < abs(torque_from_error):
+        self._pid_log.error = self._pid_log.error * (1.0 - error_blend_factor) + torque_from_error * error_blend_factor
+
+    # compute feedforward (same as nn setpoint output)
+    friction_input = self.update_friction_input(self._setpoint, self._measurement)
+    nn_input = [CS.vEgo, self._desired_lateral_accel, friction_input, roll] \
+               + past_lateral_accels_desired + future_planned_lateral_accels \
+               + past_rolls + future_rolls
+    self._ff = self.model.evaluate(nn_input)
+
+    # apply friction override for cars with low NN friction response
+    if self.model.friction_override:
+      self._pid_log.error += get_friction(friction_input, self._lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
+
+    self.update_output_torque(CS)

openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_fingerprint.py

@@ -0,0 +1,43 @@
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.honda.values import CAR as HONDA
+from opendbc.car.hyundai.values import CAR as HYUNDAI
+from opendbc.car.nissan.values import CAR as NISSAN
+from opendbc.car.toyota.values import CAR as TOYOTA
+from opendbc.car.tesla.values import CAR as TESLA
+from openpilot.common.parameterized import parameterized
+from openpilot.common.params import Params
+from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
+
+
+FINGERPRINT_EXACT_MATCH = [HONDA.HONDA_CIVIC_BOSCH, TOYOTA.TOYOTA_RAV4_TSS2_2022, HYUNDAI.HYUNDAI_IONIQ_5]
+FINGERPRINT_FUZZY_MATCH = [HONDA.HONDA_CIVIC_BOSCH_DIESEL, HYUNDAI.GENESIS_G70_2020, HYUNDAI.HYUNDAI_IONIQ_6]
+FINGERPRINT_ANGLE_NO_MATCH = [TOYOTA.TOYOTA_RAV4_TSS2_2023, NISSAN.NISSAN_LEAF, TESLA.TESLA_MODEL_3]
+
+
+class TestNNLCFingerprintBase:
+
+  @staticmethod
+  def _setup_platform(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, Params())
+
+    return CI
+
+  @parameterized.expand(FINGERPRINT_EXACT_MATCH)
+  def test_exact_fingerprint(self, car_name):
+    CI = self._setup_platform(car_name)
+    assert CI.CP_SP.neuralNetworkLateralControl.model.name != "MOCK" and not CI.CP_SP.neuralNetworkLateralControl.fuzzyFingerprint
+
+  @parameterized.expand(FINGERPRINT_FUZZY_MATCH)
+  def test_fuzzy_fingerprint(self, car_name):
+    CI = self._setup_platform(car_name)
+    assert CI.CP_SP.neuralNetworkLateralControl.model.name != "MOCK" and CI.CP_SP.neuralNetworkLateralControl.fuzzyFingerprint
+
+  @parameterized.expand(FINGERPRINT_ANGLE_NO_MATCH)
+  def test_no_fingerprint(self, car_name):
+    CI = self._setup_platform(car_name)
+    assert CI.CP_SP.neuralNetworkLateralControl.model.name == "MOCK"

openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_load_model.py

@@ -0,0 +1,31 @@
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.honda.values import CAR as HONDA
+from opendbc.car.hyundai.values import CAR as HYUNDAI
+from opendbc.car.toyota.values import CAR as TOYOTA
+from openpilot.common.parameterized import parameterized
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.car.helpers import convert_to_capnp
+from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
+
+
+class TestNNTorqueModel:
+
+  @parameterized.expand([HONDA.HONDA_CIVIC, TOYOTA.TOYOTA_RAV4, HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN])
+  def test_load_model(self, car_name):
+    params = Params()
+    params.put_bool("NeuralNetworkLateralControl", True, block=True)
+
+    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, params)
+
+    CP_SP = convert_to_capnp(CP_SP)
+
+    controller = LatControlTorque(CP.as_reader(), CP_SP.as_reader(), CI, DT_CTRL)
+
+    assert controller.extension.has_nn_model

openpilot/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py

@@ -0,0 +1,103 @@
+import numpy as np
+
+from openpilot.cereal import log, messaging
+from opendbc.car.structs import car
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.gm.values import CAR as GM
+from opendbc.car.honda.values import CAR as HONDA
+from opendbc.car.hyundai.values import CAR as HYUNDAI
+from opendbc.car.toyota.values import CAR as TOYOTA
+from opendbc.car.vehicle_model import VehicleModel
+from openpilot.common.parameterized import parameterized
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.car.helpers import convert_to_capnp
+from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from openpilot.selfdrive.locationd.helpers import Pose
+from openpilot.common.mock.generators import generate_livePose
+from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
+from openpilot.selfdrive.modeld.constants import ModelConstants
+
+
+def generate_modelV2():
+  model = messaging.new_message('modelV2')
+  position = log.XYZTData.new_message()
+  speed = 30
+  position.x = [float(x) for x in (speed + 0.5) * np.array(ModelConstants.T_IDXS)]
+  model.modelV2.position = position
+  orientation = log.XYZTData.new_message()
+  curvature = 0.05
+  orientation.x = [float(curvature) for _ in ModelConstants.T_IDXS]
+  orientation.y = [0.0 for _ in ModelConstants.T_IDXS]
+  model.modelV2.orientation = orientation
+  velocity = log.XYZTData.new_message()
+  velocity.x = [float(x) for x in (speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
+  velocity.x[0] = float(speed)  # always start at current speed
+  model.modelV2.velocity = velocity
+  acceleration = log.XYZTData.new_message()
+  acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
+  acceleration.y = [float(y) for y in np.zeros_like(ModelConstants.T_IDXS)]
+  model.modelV2.acceleration = acceleration
+
+  return model
+
+
+class TestNeuralNetworkLateralControl:
+
+  @parameterized.expand([HONDA.HONDA_CIVIC, TOYOTA.TOYOTA_RAV4, HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN, GM.CHEVROLET_BOLT_EUV])
+  def test_saturation(self, car_name):
+    params = Params()
+    params.put_bool("NeuralNetworkLateralControl", True, block=True)
+
+    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, params)
+
+    CP_SP = convert_to_capnp(CP_SP)
+    VM = VehicleModel(CP)
+
+    controller = LatControlTorque(CP.as_reader(), CP_SP.as_reader(), CI, DT_CTRL)
+    torque_params = CP.lateralTuning.torque
+
+    CS = car.CarState.new_message()
+    CS.vEgo = 30
+    CS.steeringPressed = False
+
+    params = log.LiveParametersData.new_message()
+
+    lp = generate_livePose()
+    pose = Pose.from_live_pose(lp.livePose)
+
+    mdl = generate_modelV2()
+    sm = {'modelV2': mdl.modelV2}
+    model_v2 = sm['modelV2']
+    controller.extension.model_v2 = model_v2
+
+    # Saturate for curvature limited and controller limited
+    test_lag = 0.3
+    for _ in range(1000):
+      controller.extension.update_model_v2(model_v2)
+      controller.extension.update_lateral_lag(test_lag)
+      controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
+      controller.extension.update_limits()
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True, 0.2)
+    assert lac_log.saturated
+
+    for _ in range(1000):
+      controller.extension.update_model_v2(model_v2)
+      controller.extension.update_lateral_lag(test_lag)
+      controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
+      controller.extension.update_limits()
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False, 0.2)
+    assert not lac_log.saturated
+
+    for _ in range(1000):
+      controller.extension.update_model_v2(model_v2)
+      controller.extension.update_lateral_lag(test_lag)
+      controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction)
+      controller.extension.update_limits()
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False, 0.2)
+    assert lac_log.saturated

openpilot/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

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

@@ -0,0 +1,261 @@
+import json
+import math
+import platform
+
+from openpilot.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

openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/smart_cruise_control.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.
+"""
+import openpilot.cereal.messaging as messaging
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.map_controller import SmartCruiseControlMap
+
+
+class SmartCruiseControl:
+  def __init__(self):
+    self.vision = SmartCruiseControlVision()
+    self.map = SmartCruiseControlMap()
+
+  def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise: float) -> None:
+    self.map.update(long_enabled, long_override, v_ego, a_ego, v_cruise)
+    self.vision.update(sm, long_enabled, long_override, v_ego, a_ego, v_cruise)

openpilot/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 openpilot.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, block=True)
+
+    # TODO-SP: mock data from gpsLocation
+    self.params.put("LastGPSPosition", "{}", block=True)
+    self.params.put("MapTargetVelocities", "{}", block=True)
+
+  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, block=True)
+    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

openpilot/sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py

@@ -0,0 +1,214 @@
+"""
+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
+import pytest
+
+import openpilot.cereal.messaging as messaging
+from openpilot.cereal import custom, log
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision, _ENTERING_PRED_LAT_ACC_TH
+
+VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
+
+
+def _th_above_f32(th: float) -> float:
+  """
+  Return the next representable float32 *above* `th`.
+  This avoids flaky comparisons around thresholds due to float32 rounding.
+  """
+  th32 = np.float32(th)
+  above32 = np.nextafter(th32, np.float32(np.inf), dtype=np.float32)
+  return float(above32)
+
+
+def _build_single_spike_filtered(n: int, base: float = 1.0) -> np.ndarray:
+  """
+  Create an array where max() is >= threshold but p97 is < threshold.
+  This demonstrates the behavior difference vs np.amax().
+
+  Note: We intentionally construct using float32-representable values to match
+  the data path through cereal/capnp.
+  """
+  th = float(_ENTERING_PRED_LAT_ACC_TH)
+  th32 = float(np.float32(th))
+
+  # numpy percentile default is linear interpolation: idx=(n-1)*p/100
+  idx = (n - 1) * 0.97
+  w = float(idx - np.floor(idx))
+
+  base32 = float(np.float32(base))
+
+  # Choose spike so that p97 = base + w*(spike-base) < th
+  # -> spike < base + (th-base)/w. Use a margin (0.9) and ensure spike >= th.
+  if w == 0.0:
+    spike = th32 + 1.0
+  else:
+    spike = base32 + (th32 - base32) / w * 0.9
+    spike = max(spike, th32 + 0.01)
+
+  arr = np.full(n, base32, dtype=np.float32)
+  arr[-1] = np.float32(spike)
+  return arr
+
+
+def generate_modelV2():
+  model = messaging.new_message('modelV2')
+  position = log.XYZTData.new_message()
+  speed = 30
+  position.x = [float(x) for x in (speed + 0.5) * np.array(ModelConstants.T_IDXS)]
+  model.modelV2.position = position
+  orientation = log.XYZTData.new_message()
+  curvature = 0.05
+  orientation.x = [float(curvature) for _ in ModelConstants.T_IDXS]
+  orientation.y = [0.0 for _ in ModelConstants.T_IDXS]
+  model.modelV2.orientation = orientation
+  orientationRate = log.XYZTData.new_message()
+  orientationRate.z = [float(z) for z in ModelConstants.T_IDXS]
+  model.modelV2.orientationRate = orientationRate
+  velocity = log.XYZTData.new_message()
+  velocity.x = [float(x) for x in (speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
+  velocity.x[0] = float(speed)  # always start at current speed
+  model.modelV2.velocity = velocity
+  acceleration = log.XYZTData.new_message()
+  acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
+  acceleration.y = [float(y) for y in np.zeros_like(ModelConstants.T_IDXS)]
+  model.modelV2.acceleration = acceleration
+
+  return model
+
+
+def generate_carState():
+  car_state = messaging.new_message('carState')
+  speed = 30
+  v_cruise = 50
+  car_state.carState.vEgo = float(speed)
+  car_state.carState.standstill = False
+  car_state.carState.vCruise = float(v_cruise * 3.6)
+
+  return car_state
+
+
+def generate_controlsState():
+  controls_state = messaging.new_message('controlsState')
+  controls_state.controlsState.curvature = 0.05
+
+  return controls_state
+
+
+class TestSmartCruiseControlVision:
+
+  def setup_method(self):
+    self.params = Params()
+    self.reset_params()
+    self.scc_v = SmartCruiseControlVision()
+
+    mdl = generate_modelV2()
+    cs = generate_carState()
+    controls_state = generate_controlsState()
+    self.sm = {'modelV2': mdl.modelV2, 'carState': cs.carState, 'controlsState': controls_state.controlsState}
+
+  def reset_params(self):
+    self.params.put_bool("SmartCruiseControlVision", True, block=True)
+
+  def test_initial_state(self):
+    assert self.scc_v.state == VisionState.disabled
+    assert not self.scc_v.is_active
+    assert self.scc_v.output_v_target == V_CRUISE_UNSET
+    assert self.scc_v.output_a_target == 0.
+
+  def test_system_disabled(self):
+    self.params.put_bool("SmartCruiseControlVision", False, block=True)
+    self.scc_v.enabled = self.params.get_bool("SmartCruiseControlVision")
+
+    for _ in range(int(10. / DT_MDL)):
+      self.scc_v.update(self.sm, True, False, 0., 0., 0.)
+    assert self.scc_v.state == VisionState.disabled
+    assert not self.scc_v.is_active
+
+  def test_disabled(self):
+    for _ in range(int(10. / DT_MDL)):
+      self.scc_v.update(self.sm, False, False, 0., 0., 0.)
+    assert self.scc_v.state == VisionState.disabled
+
+  def test_transition_disabled_to_enabled(self):
+    for _ in range(int(10. / DT_MDL)):
+      self.scc_v.update(self.sm, True, False, 0., 0., 0.)
+    assert self.scc_v.state == VisionState.enabled
+
+  @pytest.mark.parametrize(
+    "case, should_enter",
+    [
+      ("p97_just_above_threshold", True),
+      ("single_spike_filtered", False),
+      ("persistent_high_values", True),
+    ],
+    ids=[
+      "p97>threshold_enters",
+      "single_spike_max_large_but_p97_below_threshold",
+      "high_values_persist_trigger_entering",
+    ],
+  )
+  def test_max_pred_lat_acc_uses_p97_and_threshold(self, case, should_enter):
+    n = len(ModelConstants.T_IDXS)
+    th = float(_ENTERING_PRED_LAT_ACC_TH)
+
+    if case == "p97_just_above_threshold":
+      # Use the next representable float32 above threshold to avoid float32 rounding flakiness.
+      val = _th_above_f32(th)
+      pred_lat_accels = np.full(n, np.float32(val), dtype=np.float32)
+
+    elif case == "single_spike_filtered":
+      pred_lat_accels = _build_single_spike_filtered(n, base=1.0)
+
+    elif case == "persistent_high_values":
+      # Make enough "high" samples so p97 is driven by the persistent trend, not a single outlier.
+      high_count = max(2, int(np.ceil(n * 0.03)) + 1)
+      pred_lat_accels = np.full(n, np.float32(1.0), dtype=np.float32)
+      pred_lat_accels[-high_count:] = np.float32(2.0)
+      pred_lat_accels[-1] = np.float32(8.0)  # keep one big outlier too
+
+    else:
+      raise AssertionError(f"Unknown case: {case}")
+
+    # Override model predictions so:
+    # predicted_lat_accels = abs(orientationRate.z) * velocity.x == pred_lat_accels
+    mdl = generate_modelV2()
+    mdl.modelV2.velocity.x = [1.0 for _ in range(n)]
+    mdl.modelV2.orientationRate.z = [float(x) for x in pred_lat_accels]
+    self.sm["modelV2"] = mdl.modelV2
+
+    v_ego = float(MIN_V + 5.0)
+
+    # 1st update: disabled -> enabled
+    self.scc_v.update(self.sm, True, False, v_ego, 0.0, 0.0)
+    # 2nd update: evaluate entering condition from enabled state
+    self.scc_v.update(self.sm, True, False, v_ego, 0.0, 0.0)
+
+    # Controller does percentile on numpy float64 arrays (values already quantized by capnp),
+    # so compute expected in float64 to match behavior and avoid interpolation/rounding deltas.
+    expected_p97 = float(np.percentile(pred_lat_accels.astype(np.float64), 97))
+
+    # allow tiny numeric differences due to float conversions/interpolation
+    assert np.isclose(self.scc_v.max_pred_lat_acc, expected_p97, rtol=1e-6, atol=1e-5)
+
+    if should_enter:
+      # We assert entering primarily by state (this is the actual intended behavior).
+      assert self.scc_v.state == VisionState.entering
+      # Optional sanity: should be >= threshold with some margin (since we used nextafter above threshold).
+      assert self.scc_v.max_pred_lat_acc > th
+
+    else:
+      # Difference vs np.amax(): max can be above threshold, but p97 stays below it.
+      assert float(np.max(pred_lat_accels)) >= th
+      assert self.scc_v.max_pred_lat_acc < th
+      assert self.scc_v.state == VisionState.enabled
+
+  # TODO-SP: mock modelV2 data to test other states

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

@@ -0,0 +1,203 @@
+"""
+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
+
+import openpilot.cereal.messaging as messaging
+from openpilot.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.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)
+
+_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.
+
+_TURNING_LAT_ACC_TH = 1.6  # Lat Acc threshold to trigger turning state.
+
+_LEAVING_LAT_ACC_TH = 1.3  # Lat Acc threshold to trigger leaving turn state.
+_FINISH_LAT_ACC_TH = 1.1  # Lat Acc threshold to trigger the end of the turn cycle.
+
+_A_LAT_REG_MAX = 2.  # Maximum lateral acceleration
+
+_NO_OVERSHOOT_TIME_HORIZON = 4.  # s. Time to use for velocity desired based on a_target when not overshooting.
+
+# Lookup table for the minimum smooth deceleration during the ENTERING state
+# depending on the actual maximum absolute lateral acceleration predicted on the turn ahead.
+_ENTERING_SMOOTH_DECEL_V = [-0.2, -1.]  # min decel value allowed on ENTERING state
+_ENTERING_SMOOTH_DECEL_BP = [1.3, 3.]  # absolute value of lat acc ahead
+
+# Lookup table for the acceleration for the TURNING state
+# depending on the current lateral acceleration of the vehicle.
+_TURNING_ACC_V = [0.5, 0., -0.4]  # acc value
+_TURNING_ACC_BP = [1.5, 2.3, 3.]  # absolute value of current lat acc
+
+_LEAVING_ACC = 0.5  # Conformable acceleration to regain speed while leaving a turn.
+
+
+class SmartCruiseControlVision:
+  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.frame = -1
+    self.long_enabled = False
+    self.long_override = False
+    self.is_enabled = False
+    self.is_active = False
+    self.enabled = self.params.get_bool("SmartCruiseControlVision")
+    self.v_cruise_setpoint = 0.
+
+    self.state = VisionState.disabled
+    self.current_lat_acc = 0.
+    self.max_pred_lat_acc = 0.
+
+  def get_a_target_from_control(self) -> float:
+    return self.a_target
+
+  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 V_CRUISE_UNSET
+
+  def _update_params(self) -> None:
+    if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0:
+      self.enabled = self.params.get_bool("SmartCruiseControlVision")
+
+  def _update_calculations(self, sm: messaging.SubMaster) -> None:
+    if not self.long_enabled:
+      return
+    else:
+      rate_plan = np.array(np.abs(sm['modelV2'].orientationRate.z))
+      vel_plan = np.array(sm['modelV2'].velocity.x)
+
+      self.current_lat_acc = self.v_ego ** 2 * abs(sm['controlsState'].curvature)
+
+      # get the maximum lat accel from the model
+      predicted_lat_accels = rate_plan * vel_plan
+      self.max_pred_lat_acc = np.percentile(predicted_lat_accels, 97)
+
+      # get the maximum curve based on the current velocity
+      v_ego = max(self.v_ego, 0.1)  # ensure a value greater than 0 for calculations
+      max_curve = self.max_pred_lat_acc / (v_ego**2)
+
+      # Get the target velocity for the maximum curve
+      self.v_target = (_A_LAT_REG_MAX / max_curve) ** 0.5
+
+  def _update_state_machine(self) -> tuple[bool, bool]:
+    # ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING
+    if self.state != VisionState.disabled:
+      # longitudinal and feature disable always have priority in a non-disabled state
+      if not self.long_enabled or not self.enabled:
+        self.state = VisionState.disabled
+      elif self.long_override:
+        self.state = VisionState.overriding
+
+      else:
+        # ENABLED
+        if self.state == VisionState.enabled:
+          # Do not enter a turn control cycle if the speed is low.
+          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:
+            self.state = VisionState.entering
+
+        # OVERRIDING
+        elif self.state == VisionState.overriding:
+          if not self.long_override:
+            self.state = VisionState.enabled
+
+        # ENTERING
+        elif self.state == VisionState.entering:
+          # Transition to Turning if current lateral acceleration is over the threshold.
+          if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
+            self.state = VisionState.turning
+          # Abort if the predicted lateral acceleration drops
+          elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH:
+            self.state = VisionState.enabled
+
+        # TURNING
+        elif self.state == VisionState.turning:
+          # Transition to Leaving if current lateral acceleration drops below a threshold.
+          if self.current_lat_acc <= _LEAVING_LAT_ACC_TH:
+            self.state = VisionState.leaving
+
+        # LEAVING
+        elif self.state == VisionState.leaving:
+          # Transition back to Turning if current lateral acceleration goes back over the threshold.
+          if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
+            self.state = VisionState.turning
+          # Finish if current lateral acceleration goes below a threshold.
+          elif self.current_lat_acc < _FINISH_LAT_ACC_TH:
+            self.state = VisionState.enabled
+
+    # DISABLED
+    elif self.state == VisionState.disabled:
+      if self.long_enabled and self.enabled:
+        if self.long_override:
+          self.state = VisionState.overriding
+        else:
+          self.state = VisionState.enabled
+
+    enabled = self.state in ENABLED_STATES
+    active = self.state in ACTIVE_STATES
+
+    return enabled, active
+
+  def _update_solution(self) -> float:
+    # DISABLED, ENABLED, OVERRIDING
+    if self.state not in ACTIVE_STATES:
+      # when not overshooting, calculate v_turn as the speed at the prediction horizon when following
+      # the smooth deceleration.
+      a_target = self.a_ego
+    # ENTERING
+    elif self.state == VisionState.entering:
+      # when not overshooting, target a smooth deceleration in preparation for a sharp turn to come.
+      a_target = np.interp(self.max_pred_lat_acc, _ENTERING_SMOOTH_DECEL_BP, _ENTERING_SMOOTH_DECEL_V)
+    # TURNING
+    elif self.state == VisionState.turning:
+      # When turning, we provide a target acceleration that is comfortable for the lateral acceleration felt.
+      a_target = np.interp(self.current_lat_acc, _TURNING_ACC_BP, _TURNING_ACC_V)
+    # LEAVING
+    elif self.state == VisionState.leaving:
+      # When leaving, we provide a comfortable acceleration to regain speed.
+      a_target = _LEAVING_ACC
+    else:
+      raise NotImplementedError(f"SCC-V state not supported: {self.state}")
+
+    return a_target
+
+  def update(self, sm: messaging.SubMaster, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float,
+             v_cruise_setpoint: float) -> None:
+    self.long_enabled = long_enabled
+    self.long_override = long_override
+    self.v_ego = v_ego
+    self.a_ego = a_ego
+    self.v_cruise_setpoint = v_cruise_setpoint
+
+    self._update_params()
+    self._update_calculations(sm)
+
+    self.is_enabled, self.is_active = self._update_state_machine()
+    self.a_target = self._update_solution()
+
+    self.output_v_target = self.get_v_target_from_control()
+    self.output_a_target = self.get_a_target_from_control()
+
+    self.frame += 1

openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/__init__.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.
+"""
+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
+}

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

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

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

@@ -0,0 +1,45 @@
+"""
+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.cereal import custom
+from opendbc.car.structs import car
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode as SpeedLimitMode
+
+
+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
+
+
+def set_speed_limit_assist_availability(CP: car.CarParams, CP_SP: custom.CarParamsSP, params: Params = None) -> bool:
+  if params is None:
+    params = Params()
+
+  is_release = params.get_bool("IsReleaseSpBranch")
+  disallow_in_release = CP.brand == "tesla" and is_release
+  always_disallow = CP.brand == "rivian"
+  allowed = True
+
+  if disallow_in_release or always_disallow:
+    allowed = False
+
+  if not CP.openpilotLongitudinalControl and CP_SP.pcmCruiseSpeed:
+    allowed = False
+
+  if not allowed:
+    if params.get("SpeedLimitMode", return_default=True) == SpeedLimitMode.assist:
+      params.put("SpeedLimitMode", int(SpeedLimitMode.warning), block=True)
+
+  return allowed

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

@@ -0,0 +1,415 @@
+"""
+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 openpilot.cereal import custom
+from opendbc.car.structs import car
+from openpilot.common.params import Params
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
+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, set_speed_limit_assist_availability
+
+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.
+# secs. Time to wait after activation before considering temp deactivation signal.
+PRE_ACTIVE_GUARD_PERIOD = {
+  True: 15,
+  False: 5,
+}
+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: car.CarParams, CP_SP: custom.CarParamsSP):
+    self.params = Params()
+    self.CP = CP
+    self.CP_SP = CP_SP
+    self.frame = -1
+    self.long_engaged_timer = 0
+    self.pre_active_timer = 0
+    self.is_metric = self.params.get_bool("IsMetric")
+    set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params)
+    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)
+
+  @property
+  def v_cruise_cluster_below_confirm_speed_threshold(self) -> bool:
+    return bool(self.v_cruise_cluster_conv < CONFIRM_SPEED_THRESHOLD[self.is_metric])
+
+  def update_active_event(self, events_sp: EventsSP) -> None:
+    if self.v_cruise_cluster_below_confirm_speed_threshold:
+      events_sp.add(EventNameSP.speedLimitChanged)
+    else:
+      events_sp.add(EventNameSP.speedLimitActive)
+
+  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")
+      set_speed_limit_assist_availability(self.CP, self.CP_SP, self.params)
+      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_below_confirm_speed_threshold:
+      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[self.pcm_op_long] / 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[self.pcm_op_long] / 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[self.pcm_op_long] / 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[self.pcm_op_long] / 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[self.pcm_op_long] / 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[self.pcm_op_long] / 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[self.pcm_op_long] / 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:
+        self.update_active_event(events_sp)
+
+      # 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:
+          self.update_active_event(events_sp)
+        elif self.speed_limit_prev > 0 and self._speed_limit > 0:
+          self.update_active_event(events_sp)
+
+  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

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

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

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

@@ -0,0 +1,278 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import pytest
+
+from openpilot.cereal import custom
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.rivian.values import CAR as RIVIAN
+from opendbc.car.tesla.values import CAR as TESLA
+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 import PARAMS_UPDATE_PERIOD
+from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Mode
+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
+}
+
+DEFAULT_CAR = TOYOTA.TOYOTA_RAV4_TSS2
+
+
+@pytest.fixture
+def car_name(request):
+  return getattr(request, "param", DEFAULT_CAR)
+
+
+@pytest.fixture(autouse=True)
+def set_car_name_on_instance(request, car_name):
+  instance = getattr(request, "instance", None)
+  if instance:
+    instance.car_name = car_name
+
+
+class TestSpeedLimitAssist:
+
+  def setup_method(self, method):
+    self.params = Params()
+    self.reset_custom_params()
+    self.events_sp = EventsSP()
+    CI = self._setup_platform(self.car_name)
+    self.sla = SpeedLimitAssist(CI.CP, CI.CP_SP)
+    self.sla.pre_active_timer = int(PRE_ACTIVE_GUARD_PERIOD[self.sla.pcm_op_long] / 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)
+    CI.CP.openpilotLongitudinalControl = True  # always assume it's openpilot longitudinal
+    sunnypilot_interfaces.setup_interfaces(CI, self.params)
+    return CI
+
+  def reset_custom_params(self):
+    self.params.put("IsReleaseSpBranch", True, block=True)
+    self.params.put("SpeedLimitMode", int(Mode.assist), block=True)
+    self.params.put_bool("IsMetric", False, block=True)
+    self.params.put("SpeedLimitOffsetType", 0, block=True)
+    self.params.put("SpeedLimitValueOffset", 0, block=True)
+
+  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()
+
+  @pytest.mark.parametrize("car_name", [RIVIAN.RIVIAN_R1, TESLA.TESLA_MODEL_Y], indirect=True)
+  def test_disallowed_brands(self, car_name):
+    """
+      Speed Limit Assist is disabled for the following brands and conditions:
+      - All Tesla and is a release branch;
+      - All Rivian
+    """
+    assert not self.sla.enabled
+
+    # stay disallowed even when the param may have changed from somewhere else
+    self.params.put("SpeedLimitMode", int(Mode.assist), block=True)
+    for _ in range(int(PARAMS_UPDATE_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 not self.sla.enabled
+
+  def test_disabled(self):
+    self.params.put("SpeedLimitMode", int(Mode.off), block=True)
+    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[self.sla.pcm_op_long] / 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

openpilot/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_resolver.py

@@ -0,0 +1,144 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import random
+import time
+
+import pytest
+from pytest_mock import MockerFixture
+
+from openpilot.cereal import custom
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import LIMIT_MAX_MAP_DATA_AGE
+
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver, ALL_SOURCES
+from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.common import Policy
+
+SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source
+
+
+def create_mock(properties, mocker: MockerFixture):
+  mock = mocker.MagicMock()
+  for _property, value in properties.items():
+    setattr(mock, _property, value)
+  return mock
+
+
+def setup_sm_mock(mocker: MockerFixture):
+  cruise_speed_limit = random.uniform(0, 120)
+  live_map_data_limit = random.uniform(0, 120)
+
+  car_state = create_mock({
+    'gasPressed': False,
+    'brakePressed': False,
+    'standstill': False,
+  }, mocker)
+  car_state_sp = create_mock({
+    'speedLimit': cruise_speed_limit,
+  }, mocker)
+  live_map_data = create_mock({
+    'speedLimit': live_map_data_limit,
+    'speedLimitValid': True,
+    'speedLimitAhead': 0.,
+    'speedLimitAheadValid': 0.,
+    'speedLimitAheadDistance': 0.,
+  }, mocker)
+  gps_data = create_mock({
+    'unixTimestampMillis': time.monotonic() * 1e3,
+  }, mocker)
+  sm_mock = mocker.MagicMock()
+  sm_mock.__getitem__.side_effect = lambda key: {
+    'carState': car_state,
+    'liveMapDataSP': live_map_data,
+    'carStateSP': car_state_sp,
+    'gpsLocation': gps_data,
+  }[key]
+  return sm_mock
+
+
+parametrized_policies = pytest.mark.parametrize(
+  "policy, sm_key, function_key", [
+    (Policy.car_state_only, 'carStateSP', SpeedLimitSource.car),
+    (Policy.car_state_priority, 'carStateSP', SpeedLimitSource.car),
+    (Policy.map_data_only, 'liveMapDataSP', SpeedLimitSource.map),
+    (Policy.map_data_priority, 'liveMapDataSP', SpeedLimitSource.map),
+  ],
+  ids=lambda val: val.name if hasattr(val, 'name') else str(val)
+)
+
+
+@pytest.mark.parametrize("resolver_class", [SpeedLimitResolver])
+class TestSpeedLimitResolverValidation:
+
+  @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
+  def test_initial_state(self, resolver_class, policy):
+    resolver = resolver_class()
+    resolver.policy = policy
+    for source in ALL_SOURCES:
+      if source in resolver.limit_solutions:
+        assert resolver.limit_solutions[source] == 0.
+        assert resolver.distance_solutions[source] == 0.
+
+  @parametrized_policies
+  def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
+    resolver = resolver_class()
+    resolver.policy = policy
+    sm_mock = setup_sm_mock(mocker)
+    source_speed_limit = sm_mock[sm_key].speedLimit
+
+    # Assert the resolver
+    resolver.update(source_speed_limit, sm_mock)
+    assert resolver.speed_limit == source_speed_limit
+    assert resolver.source == ALL_SOURCES[function_key]
+
+  def test_resolver_combined(self, resolver_class, mocker: MockerFixture):
+    resolver = resolver_class()
+    resolver.policy = Policy.combined
+    sm_mock = setup_sm_mock(mocker)
+    socket_to_source = {'carStateSP': SpeedLimitSource.car, 'liveMapDataSP': SpeedLimitSource.map}
+    minimum_key, minimum_speed_limit = min(
+      ((key, sm_mock[key].speedLimit) for key in
+       socket_to_source.keys()), key=lambda x: x[1])
+
+    # Assert the resolver
+    resolver.update(minimum_speed_limit, sm_mock)
+    assert resolver.speed_limit == minimum_speed_limit
+    assert resolver.source == socket_to_source[minimum_key]
+
+  @parametrized_policies
+  def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture):
+    resolver = resolver_class()
+    resolver.policy = policy
+    sm_mock = setup_sm_mock(mocker)
+    source_speed_limit = sm_mock[sm_key].speedLimit
+
+    # Assert the parsing
+    resolver.update(source_speed_limit, sm_mock)
+    assert resolver.limit_solutions[ALL_SOURCES[function_key]] == source_speed_limit
+    assert resolver.distance_solutions[ALL_SOURCES[function_key]] == 0.
+
+  @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
+  def test_resolve_interaction_in_update(self, resolver_class, policy, mocker: MockerFixture):
+    v_ego = 50
+    resolver = resolver_class()
+    resolver.policy = policy
+
+    sm_mock = setup_sm_mock(mocker)
+    resolver.update(v_ego, sm_mock)
+
+    # After resolution
+    assert resolver.speed_limit is not None
+    assert resolver.distance is not None
+    assert resolver.source is not None
+
+  @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name)
+  def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture):
+    resolver = resolver_class()
+    resolver.policy = policy
+    sm_mock = mocker.MagicMock()
+    sm_mock['gpsLocation'].unixTimestampMillis = (time.monotonic() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3
+    resolver._get_from_map_data(sm_mock)
+    assert resolver.limit_solutions[SpeedLimitSource.map] == 0.
+    assert resolver.distance_solutions[SpeedLimitSource.map] == 0.

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_auto_lane_change.py

@@ -0,0 +1,211 @@
+"""
+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.parameterized import parameterized
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState, LaneChangeDirection
+from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode, \
+  AUTO_LANE_CHANGE_TIMER, ONE_SECOND_DELAY
+
+AUTO_LANE_CHANGE_TIMER_COMBOS = [
+  (AutoLaneChangeMode.NUDGELESS, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGELESS]),
+  (AutoLaneChangeMode.HALF_SECOND, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.HALF_SECOND]),
+  (AutoLaneChangeMode.ONE_SECOND, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.ONE_SECOND]),
+  (AutoLaneChangeMode.TWO_SECONDS, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.TWO_SECONDS]),
+  (AutoLaneChangeMode.THREE_SECONDS, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.THREE_SECONDS])
+]
+
+
+class TestAutoLaneChangeController:
+  def setup_method(self):
+    self.DH = DesireHelper()
+    self.alc = AutoLaneChangeController(self.DH)
+
+  def _reset_states(self):
+    self.alc.lane_change_bsm_delay = False
+    self.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+    self.lane_change_wait_timer = 0.0
+    self.prev_brake_pressed = False
+    self.prev_lane_change = False
+
+  def test_reset(self):
+    """Test that reset correctly sets timers back to default."""
+    # Set some non-default values
+    self.alc.lane_change_wait_timer = 2.0
+    self.alc.prev_brake_pressed = True
+
+    # Set the DesireHelper to trigger a reset
+    self.DH.lane_change_state = LaneChangeState.off
+    self.DH.lane_change_direction = LaneChangeDirection.none
+
+    # Call reset
+    self.alc.reset()
+
+    # Check values were reset
+    assert self.alc.lane_change_wait_timer == 0.0
+    assert not self.alc.prev_brake_pressed
+
+  @parameterized.expand([(AutoLaneChangeMode.OFF, ), (AutoLaneChangeMode.NUDGE, )])
+
+  def test_off_and_nudge_mode(self, timer_state):
+    """Test the default OFF and NUDGE mode behavior."""
+    self._reset_states()
+    # Setup mode
+    self.alc.lane_change_bsm_delay = False  # BSM delay off
+    self.alc.lane_change_set_timer = timer_state
+
+    # Update controller
+    num_updates = int(5.0 / DT_MDL)
+    for _ in range(num_updates):  # Run for 5 seconds
+      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+    # Mode should not allow lane change immediately
+    assert not self.alc.auto_lane_change_allowed
+
+  def test_nudgeless_mode(self):
+    """Test the NUDGELESS mode behavior."""
+    self._reset_states()
+    # Setup NUDGELESS mode
+    self.alc.lane_change_bsm_delay = False  # BSM delay off
+    self.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS
+
+    # Update controller once to read params
+    self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+    # Update multiple times to exceed the timer threshold
+    for _ in range(1):  # Should exceed 0.1s with multiple DT_MDL updates
+      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+    # Now lane change should be allowed
+    assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay
+    assert self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_timers(self, timer_state, timer_delay):
+      self._reset_states()
+      self.alc.lane_change_bsm_delay = False  # BSM delay off
+      self.alc.lane_change_set_timer = timer_state
+
+      # Update controller once
+      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+      # The timer should still be below the threshold after one update
+      assert not self.alc.auto_lane_change_allowed
+
+      # Update enough times to exceed the threshold (seconds / DT_MDL)
+      num_updates = int(timer_delay / DT_MDL) + 1  # Add one extra updates to ensure we exceed the threshold
+      for _ in range(num_updates):
+        self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+      # Now lane change should be allowed
+      assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay
+      assert self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_brake_pressed_disables_auto_lane_change(self, timer_state, timer_delay):
+    """Test that pressing the brake disables auto lane change."""
+    self._reset_states()
+    # Setup auto lane change mode
+    self.alc.lane_change_bsm_delay = False
+    self.alc.lane_change_set_timer = timer_state
+    num_updates = int(timer_delay / DT_MDL) + 1  # Add one extra updates to ensure we exceed the threshold
+
+    # Update with brake pressed for 1 second
+    for _ in range(num_updates):
+      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=True)
+
+    # Even though it is an auto lane change mode, lane change should be disallowed due to brake pressed prior initiating lane change
+    assert not self.alc.auto_lane_change_allowed
+
+    # Check that prev_brake_pressed is saved
+    assert self.alc.prev_brake_pressed
+
+    # Even releasing brake shouldn't allow auto lane change
+    for _ in range(num_updates):
+      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+    assert not self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_blindspot_detected_with_bsm_delay(self, timer_state, timer_delay):
+    """Test behavior when blindspot is detected with BSM delay enabled."""
+    # Blindspot detected - should prevent auto lane change
+    self._reset_states()
+    self.alc.lane_change_bsm_delay = True  # BSM delay on
+    self.alc.lane_change_set_timer = timer_state
+
+    # Update with blindspot detected - this should prevent auto lane change
+    self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False)
+    assert not self.alc.auto_lane_change_allowed
+
+    # Keep updating with blindspot detected - should still prevent auto lane change
+    num_updates = int(timer_delay / DT_MDL) + 1  # Add one extra updates to ensure we exceed the threshold
+    for _ in range(num_updates):
+      self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False)
+    assert not self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_blindspot_detected_then_undetected_with_bsm_delay(self, timer_state, timer_delay):
+    """Test behavior when blindspot is detected then undetected with BSM delay enabled."""
+    # Blindspot clears - should allow auto lane change after sufficient time
+    self._reset_states()
+    self.alc.lane_change_bsm_delay = True
+    self.alc.lane_change_set_timer = timer_state
+
+    # First update with blindspot detected to set the negative timer
+    self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False)
+    assert not self.alc.auto_lane_change_allowed
+
+    # Now update with blindspot cleared - should start incrementing timer from negative value
+    num_updates = int((timer_delay + abs(ONE_SECOND_DELAY)) / DT_MDL) + 1
+    for _ in range(num_updates):
+      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+    # After sufficient updates with no blindspot, auto lane change should be allowed
+    assert self.alc.auto_lane_change_allowed
+
+  @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS)
+  def test_disallow_continuous_auto_lane_change(self, timer_state, timer_delay):
+      self._reset_states()
+      self.alc.lane_change_bsm_delay = False  # BSM delay off
+      self.alc.lane_change_set_timer = timer_state
+      num_updates = int(timer_delay / DT_MDL) + 1  # Add one extra updates to ensure we exceed the threshold
+
+      # Update enough times to exceed the threshold (seconds / DT_MDL)
+      for _ in range(num_updates):
+        self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+      # Now lane change should be allowed
+      assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay
+      assert self.alc.auto_lane_change_allowed
+
+      # Simulate lane change is initiated
+      self.DH.lane_change_state = LaneChangeState.laneChangeStarting
+      self.alc.update_state()
+
+      # Simulate lane change is completed, and one_blinker stays on
+      self.DH.lane_change_state = LaneChangeState.preLaneChange
+      self.alc.update_state()
+
+      # Update enough times to exceed the threshold (seconds / DT_MDL)
+      for _ in range(num_updates):
+        self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+      assert not self.alc.auto_lane_change_allowed
+
+  def test_auto_lane_change_mode_off_disallows_lane_change(self):
+    """Test that OFF mode never allows auto lane change."""
+    self._reset_states()
+    self.alc.lane_change_bsm_delay = False
+    self.alc.lane_change_set_timer = AutoLaneChangeMode.OFF
+
+    # Simulate updates for a long period of time (e.g., 10 seconds)
+    num_updates = int(10.0 / DT_MDL)
+    for _ in range(num_updates):
+      self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False)
+
+    # Lane change should never be allowed
+    assert not self.alc.auto_lane_change_allowed

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_blinker_pause_lateral.py

@@ -0,0 +1,144 @@
+"""
+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 opendbc.car.structs import car
+
+from openpilot.common.constants import CV
+from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral
+
+
+class TestBlinkerPauseLateral:
+
+  def setup_method(self):
+    self.blinker_pause_lateral = BlinkerPauseLateral()
+    self._reset_states()
+
+  def _reset_states(self):
+    self.blinker_pause_lateral.enabled = True
+    self.blinker_pause_lateral.is_metric = False
+    self.blinker_pause_lateral.min_speed = 20  # MPH
+    self.blinker_pause_lateral.reengage_delay = 0
+    self.blinker_pause_lateral.blinker_off_timer = 0.0
+
+    self.CS = car.CarState.new_message()
+    self.CS.vEgo = 0
+    self.CS.leftBlinker = False
+    self.CS.rightBlinker = False
+
+  def _test_should_blinker_pause_lateral(self, expected_results) -> None:
+    for left in (True, False):
+      for right in (True, False):
+        self.CS.leftBlinker = left
+        self.CS.rightBlinker = right
+
+        result = self.blinker_pause_lateral.update(self.CS)
+        assert result == expected_results[(left, right)]
+
+  def test_below_min_speed_blinker(self):
+    self.CS.vEgo = 4.5  # ~10 MPH
+
+    expected_results = {
+      (False, False): False,
+      (True, False): True,
+      (False, True): True,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_reengage_delay(self):
+    self.blinker_pause_lateral.reengage_delay = 2 # seconds
+    self.CS.vEgo = 4.5 # ~10 MPH
+
+    expected_results = {
+      (False, False): True,
+      (True, False): True,
+      (False, True): True,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_above_min_speed_blinker(self):
+    self.CS.vEgo = 13.4  # ~30 MPH
+
+    expected_results = {
+      (False, False): False,
+      (True, False): False,
+      (False, True): False,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_just_below_min_speed(self):
+    self.CS.vEgo = (20 * CV.MPH_TO_MS) - 0.01
+
+    expected_results = {
+      (False, False): False,
+      (True, False): True,
+      (False, True): True,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_disabled(self):
+    self.blinker_pause_lateral.enabled = False
+    self.CS.vEgo = 4.5  # ~10 MPH
+
+    expected_results = {
+      (False, False): False,
+      (True, False): False,
+      (False, True): False,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_metric_units_below_min_speed(self):
+    self.blinker_pause_lateral.is_metric = True
+    self.CS.vEgo = 5.0  # ~18 km/h
+
+    expected_results = {
+      (False, False): False,
+      (True, False): True,
+      (False, True): True,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_metric_units_above_threshold(self):
+    self.blinker_pause_lateral.is_metric = True
+    self.CS.vEgo = 6.0  # ~21.6 km/h
+
+    expected_results = {
+      (False, False): False,
+      (True, False): False,
+      (False, True): False,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+  def test_change_min_speed_threshold(self):
+    self.blinker_pause_lateral.min_speed = 30  # MPH
+
+    # below min speed
+    self.CS.vEgo = 11.2  # ~25 MPH
+
+    expected_results = {
+      (False, False): False,
+      (True, False): True,
+      (False, True): True,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)
+
+    # above min speed
+    self.CS.vEgo = 15.6  # ~35 MPH
+
+    expected_results = {
+      (False, False): False,
+      (True, False): False,
+      (False, True): False,
+      (True, True): False
+    }
+    self._test_should_blinker_pause_lateral(expected_results)

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py

@@ -0,0 +1,113 @@
+import pytest
+from openpilot.cereal import log, custom
+from openpilot.common.params import Params
+
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
+from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
+
+TurnDirection = custom.ModelDataV2SP.TurnDirection
+
+
+@pytest.mark.parametrize("left_blinker,right_blinker,v_ego,blindspot_left,blindspot_right,expected", [
+  (True, False, 5, False, False, TurnDirection.turnLeft),
+  (False, True, 6, False, False, TurnDirection.turnRight),
+  (True, False, 9, False, False, TurnDirection.none),
+  (True, False, 7, True, False, TurnDirection.none),
+  (False, True, 6, False, True, TurnDirection.none),
+  (False, False, 5, False, False, TurnDirection.none),
+  (True, True, 5, False, False, TurnDirection.none),
+])
+def test_lane_turn_desire_conditions(left_blinker, right_blinker, v_ego, blindspot_left, blindspot_right, expected):
+  dh = DesireHelper()
+  controller = LaneTurnController(dh)
+  controller.enabled = True
+  controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
+  controller.turn_direction = TurnDirection.none
+  controller.update_lane_turn(blindspot_left, blindspot_right, left_blinker, right_blinker, v_ego)
+  assert controller.get_turn_direction() == expected
+
+
+def test_lane_turn_desire_disabled():
+  dh = DesireHelper()
+  controller = LaneTurnController(dh)
+  controller.enabled = False
+  controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
+  controller.turn_direction = TurnDirection.none
+  controller.update_lane_turn(False, False, True, False, 7)
+  assert controller.get_turn_direction() == TurnDirection.none
+
+
+def test_lane_turn_overrides_lane_change():
+  dh = DesireHelper()
+  controller = LaneTurnController(dh)
+  controller.enabled = True
+  controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
+  controller.turn_direction = TurnDirection.none
+  # left turn desire
+  controller.update_lane_turn(False, False, True, False, 5)
+  assert controller.get_turn_direction() == TurnDirection.turnLeft
+  # right turn desire
+  controller.update_lane_turn(False, False, False, True, 6)
+  assert controller.get_turn_direction() == TurnDirection.turnRight
+  # no turn
+  controller.update_lane_turn(False, False, False, False, 7)
+  assert controller.get_turn_direction() == TurnDirection.none
+
+
+@pytest.mark.parametrize("v_ego,expected", [
+  (8.93, TurnDirection.turnLeft),  # just below threshold
+  (8.96, TurnDirection.none),  # above threshold
+  (8.95, TurnDirection.none),  # just above threshold
+])
+def test_lane_turn_desire_speed_boundary(v_ego, expected):
+  dh = DesireHelper()
+  controller = LaneTurnController(dh)
+  controller.enabled = True
+  controller.lane_turn_value = LANE_CHANGE_SPEED_MIN
+  controller.turn_direction = TurnDirection.none
+  controller.update_lane_turn(False, True, True, False, v_ego)
+  assert controller.get_turn_direction() == expected
+
+
+class DummyCarState:
+  def __init__(self, vEgo=0, leftBlinker=False, rightBlinker=False, leftBlindspot=False, rightBlindspot=False,
+               steeringPressed=False, steeringTorque=0, brakePressed=False):
+    self.vEgo = vEgo
+    self.leftBlinker = leftBlinker
+    self.rightBlinker = rightBlinker
+    self.leftBlindspot = leftBlindspot
+    self.rightBlindspot = rightBlindspot
+    self.steeringPressed = steeringPressed
+    self.steeringTorque = steeringTorque
+    self.brakePressed = brakePressed
+
+
+@pytest.fixture
+def set_lane_turn_params():
+  params = Params()
+  params.put("LaneTurnDesire", True)
+  params.put("LaneTurnValue", 20.0)
+
+
+@pytest.mark.parametrize("carstate, lateral_active, lane_change_prob, expected_desire", [
+  # Lane turn desire overrides lane change desire
+  (DummyCarState(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False), True, 1.0,
+   log.Desire.turnLeft),
+  (DummyCarState(vEgo=7, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False), True, 1.0,
+   log.Desire.turnRight),
+  # Lane change desire only (no turn desires)
+  (DummyCarState(vEgo=9, leftBlinker=True, rightBlinker=False, leftBlindspot=False, rightBlindspot=False,
+                 steeringPressed=True, steeringTorque=1), True, 1.0, log.Desire.laneChangeLeft),
+  (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=True, leftBlindspot=False, rightBlindspot=False,
+                 steeringPressed=True, steeringTorque=-1), True, 1.0, log.Desire.laneChangeRight),
+  # No desire (inactive)
+  (DummyCarState(vEgo=9, leftBlinker=False, rightBlinker=False), False, 1.0, log.Desire.none),
+  (DummyCarState(vEgo=4, leftBlinker=False, rightBlinker=False), True, 1.0, log.Desire.none),  # No blinkers? no desire!
+])
+def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
+  dh = DesireHelper()
+  dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+  for _ in range(10):
+    dh.update(carstate, lateral_active, lane_change_prob)
+  assert dh.desire == expected_desire  # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers

openpilot/sunnypilot/selfdrive/locationd/.gitignore

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

openpilot/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.which() == cereal::SensorEventData::GYRO_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.which() == cereal::SensorEventData::ACCELERATION) {
+    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();
+}

openpilot/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/params.h"
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+
+#include "sunnypilot/common/transformations/coordinates.hpp"
+#include "sunnypilot/common/transformations/orientation.hpp"
+#include "sunnypilot/system/sensord/sensors/constants.h"
+#include "sunnypilot/selfdrive/locationd/models/live_kf.h"
+
+#define VISION_DECIMATION 2
+#define SENSOR_DECIMATION 10
+#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);
+};

openpilot/sunnypilot/selfdrive/locationd/models/.gitignore

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

openpilot/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)

openpilot/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]

openpilot/sunnypilot/selfdrive/locationd/models/generated/live.h

@@ -0,0 +1,38 @@
+#pragma once
+#include "rednose/helpers/ekf.h"
+extern "C" {
+void live_update_4(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void live_update_9(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void live_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void live_update_12(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void live_update_35(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
+void live_H(double *in_vec, double *out_6746369553526897517);
+void live_err_fun(double *nom_x, double *delta_x, double *out_2356689388845686605);
+void live_inv_err_fun(double *nom_x, double *true_x, double *out_7416532235122030660);
+void live_H_mod_fun(double *state, double *out_7744797074742978621);
+void live_f_fun(double *state, double dt, double *out_8063025216892748525);
+void live_F_fun(double *state, double dt, double *out_1924980074135776075);
+void live_h_4(double *state, double *unused, double *out_7366302340587907635);
+void live_H_4(double *state, double *unused, double *out_8678952100085115055);
+void live_h_9(double *state, double *unused, double *out_1116014595619586497);
+void live_H_9(double *state, double *unused, double *out_2480573038359989091);
+void live_h_10(double *state, double *unused, double *out_15153629602772458);
+void live_H_10(double *state, double *unused, double *out_4583653000591918572);
+void live_h_12(double *state, double *unused, double *out_9071809923999704409);
+void live_H_12(double *state, double *unused, double *out_4748335565592474766);
+void live_h_35(double *state, double *unused, double *out_7011637724721731008);
+void live_H_35(double *state, double *unused, double *out_2002772533267461057);
+void live_h_32(double *state, double *unused, double *out_8005805873262674095);
+void live_H_32(double *state, double *unused, double *out_6269965131311088716);
+void live_h_13(double *state, double *unused, double *out_7868428076142185420);
+void live_H_13(double *state, double *unused, double *out_4177806446556115282);
+void live_h_14(double *state, double *unused, double *out_1116014595619586497);
+void live_H_14(double *state, double *unused, double *out_2480573038359989091);
+void live_h_33(double *state, double *unused, double *out_5108012817780654932);
+void live_H_33(double *state, double *unused, double *out_1147784471371396547);
+void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
+}

openpilot/sunnypilot/selfdrive/locationd/models/generated/live_kf_constants.h

@@ -0,0 +1,102 @@
+#pragma once
+
+#include <unordered_map>
+#include <eigen3/Eigen/Dense>
+
+#define STATE_ACCELERATION_START 16
+#define STATE_ACCELERATION_END 19
+#define STATE_ACCELERATION_LEN 3
+#define STATE_ACCELERATION_ERR_START 15
+#define STATE_ACCELERATION_ERR_END 18
+#define STATE_ACCELERATION_ERR_LEN 3
+#define STATE_ACC_BIAS_START 19
+#define STATE_ACC_BIAS_END 22
+#define STATE_ACC_BIAS_LEN 3
+#define STATE_ACC_BIAS_ERR_START 18
+#define STATE_ACC_BIAS_ERR_END 21
+#define STATE_ACC_BIAS_ERR_LEN 3
+#define STATE_ANGULAR_VELOCITY_START 10
+#define STATE_ANGULAR_VELOCITY_END 13
+#define STATE_ANGULAR_VELOCITY_LEN 3
+#define STATE_ANGULAR_VELOCITY_ERR_START 9
+#define STATE_ANGULAR_VELOCITY_ERR_END 12
+#define STATE_ANGULAR_VELOCITY_ERR_LEN 3
+#define STATE_ECEF_ORIENTATION_START 3
+#define STATE_ECEF_ORIENTATION_END 7
+#define STATE_ECEF_ORIENTATION_LEN 4
+#define STATE_ECEF_ORIENTATION_ERR_START 3
+#define STATE_ECEF_ORIENTATION_ERR_END 6
+#define STATE_ECEF_ORIENTATION_ERR_LEN 3
+#define STATE_ECEF_POS_START 0
+#define STATE_ECEF_POS_END 3
+#define STATE_ECEF_POS_LEN 3
+#define STATE_ECEF_POS_ERR_START 0
+#define STATE_ECEF_POS_ERR_END 3
+#define STATE_ECEF_POS_ERR_LEN 3
+#define STATE_ECEF_VELOCITY_START 7
+#define STATE_ECEF_VELOCITY_END 10
+#define STATE_ECEF_VELOCITY_LEN 3
+#define STATE_ECEF_VELOCITY_ERR_START 6
+#define STATE_ECEF_VELOCITY_ERR_END 9
+#define STATE_ECEF_VELOCITY_ERR_LEN 3
+#define STATE_GYRO_BIAS_START 13
+#define STATE_GYRO_BIAS_END 16
+#define STATE_GYRO_BIAS_LEN 3
+#define STATE_GYRO_BIAS_ERR_START 12
+#define STATE_GYRO_BIAS_ERR_END 15
+#define STATE_GYRO_BIAS_ERR_LEN 3
+
+#define OBSERVATION_ANGLE_OFFSET_FAST 27
+#define OBSERVATION_CAMERA_ODO_ROTATION 14
+#define OBSERVATION_CAMERA_ODO_TRANSLATION 13
+#define OBSERVATION_ECEF_ORIENTATION_FROM_GPS 32
+#define OBSERVATION_ECEF_POS 12
+#define OBSERVATION_ECEF_VEL 35
+#define OBSERVATION_FEATURE_TRACK_TEST 17
+#define OBSERVATION_GPS_NED 2
+#define OBSERVATION_GPS_VEL 5
+#define OBSERVATION_IMU_FRAME 19
+#define OBSERVATION_LANE_PT 18
+#define OBSERVATION_MSCKF_TEST 16
+#define OBSERVATION_NO_ACCEL 33
+#define OBSERVATION_NO_OBSERVATION 1
+#define OBSERVATION_NO_ROT 9
+#define OBSERVATION_ODOMETRIC_SPEED 3
+#define OBSERVATION_ORB_FEATURES 15
+#define OBSERVATION_ORB_FEATURES_WIDE 34
+#define OBSERVATION_ORB_POINT 11
+#define OBSERVATION_PHONE_ACCEL 10
+#define OBSERVATION_PHONE_GYRO 4
+#define OBSERVATION_PSEUDORANGE 22
+#define OBSERVATION_PSEUDORANGE_GLONASS 20
+#define OBSERVATION_PSEUDORANGE_GPS 6
+#define OBSERVATION_PSEUDORANGE_RATE 23
+#define OBSERVATION_PSEUDORANGE_RATE_GLONASS 21
+#define OBSERVATION_PSEUDORANGE_RATE_GPS 7
+#define OBSERVATION_ROAD_FRAME_XY_SPEED 24
+#define OBSERVATION_ROAD_FRAME_X_SPEED 30
+#define OBSERVATION_ROAD_FRAME_YAW_RATE 25
+#define OBSERVATION_ROAD_ROLL 31
+#define OBSERVATION_SPEED 8
+#define OBSERVATION_STEER_ANGLE 26
+#define OBSERVATION_STEER_RATIO 29
+#define OBSERVATION_STIFFNESS 28
+#define OBSERVATION_UNKNOWN 0
+
+static const Eigen::VectorXd live_initial_x = (Eigen::VectorXd(22) << 3.8800000e+06,-3.3700000e+06,3.7600000e+06,4.2254641e-01,-3.1238054e-01,-8.3602975e-01,-1.5788347e-01,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00).finished();
+static const Eigen::VectorXd live_initial_P_diag = (Eigen::VectorXd(21) << 1.e+02,1.e+02,1.e+02,1.e-04,1.e-04,1.e-04,1.e+02,1.e+02,1.e+02,1.e+00,1.e+00,1.e+00,1.e+00,1.e+00,1.e+00,1.e+04,1.e+04,1.e+04,1.e-04,1.e-04,1.e-04).finished();
+static const Eigen::VectorXd live_fake_gps_pos_cov_diag = (Eigen::VectorXd(3) << 1000000,1000000,1000000).finished();
+static const Eigen::VectorXd live_fake_gps_vel_cov_diag = (Eigen::VectorXd(3) << 100,100,100).finished();
+static const Eigen::VectorXd live_reset_orientation_diag = (Eigen::VectorXd(3) << 1,1,1).finished();
+static const Eigen::VectorXd live_Q_diag = (Eigen::VectorXd(21) << 8.9999999999999998e-04,8.9999999999999998e-04,8.9999999999999998e-04,9.9999999999999995e-07,9.9999999999999995e-07,9.9999999999999995e-07,1.0000000000000000e-04,1.0000000000000000e-04,1.0000000000000000e-04,1.0000000000000002e-02,1.0000000000000002e-02,1.0000000000000002e-02,2.5000000000000001e-09,2.5000000000000001e-09,2.5000000000000001e-09,9.0000000000000000e+00,9.0000000000000000e+00,9.0000000000000000e+00,2.5000000000000001e-05,2.5000000000000001e-05,2.5000000000000001e-05).finished();
+static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {
+  { 4, (Eigen::VectorXd(3) << 0.0006250000000000001,0.0006250000000000001,0.0006250000000000001).finished() },
+  { 10, (Eigen::VectorXd(3) << 0.25,0.25,0.25).finished() },
+  { 14, (Eigen::VectorXd(3) << 0.0025000000000000005,0.0025000000000000005,0.0025000000000000005).finished() },
+  { 9, (Eigen::VectorXd(3) << 2.5e-05,2.5e-05,2.5e-05).finished() },
+  { 33, (Eigen::VectorXd(3) << 0.0025000000000000005,0.0025000000000000005,0.0025000000000000005).finished() },
+  { 12, (Eigen::VectorXd(3) << 25,25,25).finished() },
+  { 35, (Eigen::VectorXd(3) << 0.25,0.25,0.25).finished() },
+  { 32, (Eigen::VectorXd(4) << 0.04000000000000001,0.04000000000000001,0.04000000000000001,0.04000000000000001).finished() },
+};
+

openpilot/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;
+}

openpilot/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;
+};

openpilot/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)

openpilot/sunnypilot/selfdrive/locationd/tests/.gitignore

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