Nuggets In Dijon model, C3xLite, livePose (#221)

* Update carrot_functions.py * fix.. atc.. * TR16 model. * fix.. atc * Adjust interpolation value for t_follow calculation * fix safeMode.. * fix safeMode2 * fix.. v_cruise * model_turn_speed.. * fix.. * fix.. * fix.. cruise.py * fix.. modelTurn.. * fix stopped car safe mode. * model turn 120% * remove model turn speed.. * paramsd... * Revert "remove model turn speed.." This reverts commit 564e9dd609. * model_turn_speed... 120 -> 115% * starting achange cost 30 -> 10 * fix.. * aChangeCostStarting * fix.. * gwm v7 * Adjust traffic stop distance parameter * Update carrot_functions.py * update gwm 250929 * trafficStopDistance adjust * localizer_roll_std * scc13 * fix... * fix.. scc13 * scc14 * bypass scc13 * fix scc13 * TheCoolPeople's Model * North Nevada Model * Revert "model_turn_speed... 120 -> 115%" This reverts commit e842a7e99f. * Reapply "remove model turn speed.." This reverts commit 544ac16811. * for c3x lite (#218) add hardware c3x lite * NNV(North Nevada) v2 * fix.. * Nuggets In Dijon Model * toyota accel pid long * for c3xlite fix (#219) * LatSmoothSec * Revert "Reapply "remove model turn speed.."" This reverts commit 2c10aae495. * apply livePose * releases 251017 --------- Co-authored-by: 「 crwusiz 」 <43285072+crwusiz@users.noreply.github.com>
2026-02-18 04:54:24 +08:00 · 2025-10-17 12:34:13 +09:00
parent b94c3eb6d9
commit 0423d12c9c
36 changed files with 792 additions and 259 deletions
--- a/RELEASES.md
+++ b/RELEASES.md
@@ -1,3 +1,10 @@
+Carrot2-v9 (2025-10-17)
+========================
+* Nuggets In Dijon model
+* Setting: Adjust traffic stop distance
+* liveLocationKalman -> livePose
+* C3x lite
+
 Carrot2-v9 (2025-09-21)
 ========================
 * GWM Model
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -1179,6 +1179,7 @@ struct ModelDataV2 {
    desire @14 :Desire;
    laneChangeProb @15 :Float32;
    desireLog @16 : Text;
+    modelTurnSpeed @17 :Float32;


    # deprecated
--- a/cereal/services.py
+++ b/cereal/services.py
@@ -51,7 +51,7 @@ _services: dict[str, tuple] = {
  "gnssMeasurements": (True, 10., 10),
  "clocks": (True, 0.1, 1),
  "ubloxRaw": (True, 20.),
-  #"livePose": (True, 20., 4),
+  "livePose": (True, 20., 4),
  "liveLocationKalman": (True, 20., 5),
  "liveParameters": (True, 20., 5),
  "cameraOdometry": (True, 20., 10),
--- a/common/mock/init.py
+++ b/common/mock/init.py
@@ -8,11 +8,12 @@ import functools
 import threading
 from cereal.messaging import PubMaster
 from cereal.services import SERVICE_LIST
-from openpilot.common.mock.generators import generate_liveLocationKalman
+from openpilot.common.mock.generators import generate_livePose, generate_liveLocationKalman
 from openpilot.common.realtime import Ratekeeper


 MOCK_GENERATOR = {
+  "livePose": generate_livePose
  "liveLocationKalman": generate_liveLocationKalman
 }

--- a/common/mock/generators.py
+++ b/common/mock/generators.py
@@ -18,3 +18,16 @@ def generate_liveLocationKalman(location=LOCATION1):
  msg.liveLocationKalman.status = 'valid'
  msg.liveLocationKalman.gpsOK = True
  return msg
+
+
+def generate_livePose():
+  msg = messaging.new_message('livePose')
+  meas = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'xStd': 0.0, 'yStd': 0.0, 'zStd': 0.0, 'valid': True}
+  msg.livePose.orientationNED = meas
+  msg.livePose.velocityDevice = meas
+  msg.livePose.angularVelocityDevice = meas
+  msg.livePose.accelerationDevice = meas
+  msg.livePose.inputsOK = True
+  msg.livePose.posenetOK = True
+  msg.livePose.sensorsOK = True
+  return msg
--- a/common/params_keys.h
+++ b/common/params_keys.h
@@ -184,6 +184,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
    {"AutoNaviCountDownMode", PERSISTENT},
    {"TurnSpeedControlMode", PERSISTENT},
    {"MapTurnSpeedFactor", PERSISTENT},
+    {"ModelTurnSpeedFactor", PERSISTENT},
    {"StoppingAccel", PERSISTENT},
    {"AutoSpeedUptoRoadSpeedLimit", PERSISTENT},
    {"AutoRoadSpeedAdjust", PERSISTENT},
@@ -208,6 +209,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
    {"MyDrivingModeAuto", PERSISTENT},
    {"TrafficLightDetectMode", PERSISTENT},
    {"SteerActuatorDelay", PERSISTENT},
+    {"LatSmoothSec", PERSISTENT},
    {"CruiseOnDist", PERSISTENT},
    {"CruiseMaxVals0", PERSISTENT},
    {"CruiseMaxVals1", PERSISTENT},
@@ -242,6 +244,8 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
    {"TFollowGap4", PERSISTENT},
    {"DynamicTFollow", PERSISTENT},
    {"DynamicTFollowLC", PERSISTENT},
+    {"AChangeCostStarting", PERSISTENT},
+    {"TrafficStopDistanceAdjust", PERSISTENT},
    {"HapticFeedbackWhenSpeedCamera", PERSISTENT},
    {"UseLaneLineSpeed", PERSISTENT},
    {"UseLaneLineCurveSpeed", PERSISTENT},
@@ -289,4 +293,6 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
    {"NNFF", PERSISTENT},
    {"NNFFLite", PERSISTENT},
    {"NNFFModelName", CLEAR_ON_OFFROAD_TRANSITION},
+
+    {"HardwareC3xLite", PERSISTENT},
 };
--- a/launch_chffrplus.sh
+++ b/launch_chffrplus.sh
@@ -90,7 +90,6 @@ function launch {
  fi

  # events language init
-  #LANG=$(cat ${PARAMS_ROOT}/d/LanguageSetting)
  LANG=$(cat /data/params/d/LanguageSetting)
  EVENTSTAT=$(git status)

@@ -102,6 +101,16 @@ function launch {
    cp -f $DIR/scripts/add/events_en.py $DIR/selfdrive/selfdrived/events.py
  fi

+  # c3xl amplifier file change
+  C3XL=$(cat /data/params/d/HardwareC3xLite)
+
+  if [ "${C3XL}" = "1" ] && [[ ! "${EVENTSTAT}" == *"modified:   system/hardware/tici/amplifier.py"* ]]; then
+    cp -f $DIR/system/hardware/tici/amplifier.py $DIR/scripts/add/amplifier_org.py
+    cp -f $DIR/scripts/add/amplifier_c3xl.py $DIR/system/hardware/tici/amplifier.py
+  elif [ "${C3XL}" = "0" ] && [[ "${EVENTSTAT}" == *"modified:   system/hardware/tici/amplifier.py"* ]]; then
+    cp -f $DIR/scripts/add/amplifier_org.py $DIR/system/hardware/tici/amplifier.py
+  fi
+
  # start manager
  cd system/manager
  if [ ! -f $DIR/prebuilt ]; then
--- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py
+++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py
@@ -355,8 +355,9 @@ class CarController(CarControllerBase):
      # 5 Hz ACC options
      if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
        if camera_scc:
-          if CS.scc13 is not None:
-            can_sends.append(hyundaican.create_acc_opt_copy(CS, self.packer))
+          #if CS.scc13 is not None:
+          #  can_sends.append(hyundaican.create_acc_opt_copy(CS, self.packer))
+          pass
        else:
          can_sends.extend(hyundaican.create_acc_opt(self.packer, self.CP))

--- a/opendbc_repo/opendbc/car/hyundai/carstate.py
+++ b/opendbc_repo/opendbc/car/hyundai/carstate.py
@@ -559,7 +559,7 @@ class CarState(CarStateBase):
      if lane_info is not None:
        left_lane_prob = lane_info["LEFT_LANE_PROB"]
        right_lane_prob = lane_info["RIGHT_LANE_PROB"]
-        left_lane_type = lane_info["LEFT_LANE_TYPE"] # 0: dashed, 1: solid, 4: double solid, solid+dashed, 5:dashed + solid
+        left_lane_type = lane_info["LEFT_LANE_TYPE"] # 0: dashed, 1: solid, 2: undecided, 3: road edge, 4: DLM Inner Solid, 5: DLM InnerDashed, 6:DLM Inner Undecided, 7: Botts Dots, 8: Barrier
        right_lane_type = lane_info["RIGHT_LANE_TYPE"]
        left_lane_color = lane_info["LEFT_LANE_COLOR"]
        right_lane_color = lane_info["RIGHT_LANE_COLOR"]
--- a/opendbc_repo/opendbc/dbc/hyundai_kia_generic.dbc
+++ b/opendbc_repo/opendbc/dbc/hyundai_kia_generic.dbc
@@ -1395,6 +1395,8 @@ BO_ 1290 SCC13: 8 SCC
 SG_ SCC_Equip : 3|1@1+ (1,0) [0|1] "" ESC
 SG_ AebDrvSetStatus : 4|3@1+ (1,0) [0|7] "" CLU,ESC
 SG_ Lead_Veh_Dep_Alert_USM : 13|2@0+ (1,0) [0|3] "" XXX
+ SG_ NEW_SIGNAL_1 : 23|8@0+ (1,0) [0|255] "" XXX
+ SG_ NEW_SIGNAL_2 : 31|8@0+ (1,0) [0|255] "" XXX

 BO_ 1287 TCS15: 4 ESC
 SG_ ABS_W_LAMP : 0|1@1+ (1.0,0.0) [0.0|1.0] ""  _4WD,CLU,CUBIS,IBOX
@@ -1489,8 +1491,10 @@ BO_ 905 SCC14: 8 SCC
 SG_ ComfortBandLower : 6|6@1+ (0.02,0) [0|1.26] "m/s^2" ESC
 SG_ JerkUpperLimit : 12|7@1+ (0.1,0) [0|12.7] "m/s^3" ESC
 SG_ JerkLowerLimit : 19|7@1+ (0.1,0) [0|12.7] "m/s^3" ESC
+ SG_ NEW_SIGNAL_1 : 26|6@1+ (1,0) [0|63] "" XXX
 SG_ ACCMode : 32|3@1+ (1,0) [0|7] "" CLU,HUD,LDWS_LKAS,ESC
 SG_ ObjDistStat : 42|2@1+ (1,0) [0|3] "" XXX
+ SG_ NEW_SIGNAL_2 : 55|8@0+ (1,0) [0|255] "" XXX
 SG_ ObjGap : 56|8@1+ (1,0) [0|255] "" CLU,HUD,ESC

 BO_ 1157 LFAHDA_MFC: 8 XXX
--- a/opendbc_repo/opendbc/safety/safety/safety_hyundai.h
+++ b/opendbc_repo/opendbc/safety/safety/safety_hyundai.h
@@ -197,6 +197,7 @@ static void hyundai_rx_hook(const CANPacket_t *to_push) {

 uint32_t last_ts_lkas11_from_op = 0;
 uint32_t last_ts_scc12_from_op = 0;
+uint32_t last_ts_scc13_from_op = 0;
 uint32_t last_ts_mdps12_from_op = 0;
 uint32_t last_ts_fca11_from_op = 0;

@@ -284,6 +285,8 @@ static bool hyundai_tx_hook(const CANPacket_t *to_send) {
    last_ts_mdps12_from_op = (tx == 0 ? 0 : microsecond_timer_get());
  else if(addr == 909)
    last_ts_fca11_from_op = (tx == 0 ? 0 : microsecond_timer_get());
+  else if(addr == 1290)
+    last_ts_scc13_from_op = (tx == 0 ? 0 : microsecond_timer_get());

  return tx;
 }
@@ -308,10 +311,11 @@ static int hyundai_fwd_hook(int bus_num, int addr) {
  if (bus_num == 2) {
    bool is_lkas_msg = addr == 832;
    bool is_lfahda_msg = addr == 1157;
-    bool is_scc_msg = addr == 1056 || addr == 1057 || addr == 1290 || addr == 905;
+    bool is_scc_msg = addr == 1056 || addr == 1057 || addr == 905;
+    bool is_scc13_msg = addr == 1290;
    bool is_fca_msg = addr == 909 || addr == 1155;

-    bool block_msg = is_lkas_msg || is_lfahda_msg || is_scc_msg; //|| is_fca_msg;
+    bool block_msg = is_lkas_msg || is_lfahda_msg || is_scc_msg || is_scc13_msg; //|| is_fca_msg;
    if (!block_msg) {
      bus_fwd = 0;
    }
@@ -321,8 +325,12 @@ static int hyundai_fwd_hook(int bus_num, int addr) {
          bus_fwd = 0;
        }
      }
-      else if(is_scc_msg) {
-        if(now - last_ts_scc12_from_op >= 400000)
+      else if (is_scc_msg) {
+        if (now - last_ts_scc12_from_op >= 400000)
+          bus_fwd = 0;
+      }
+      else if (is_scc13_msg) {
+        if (now - last_ts_scc13_from_op >= 400000)
          bus_fwd = 0;
      }
      else if(is_fca_msg) {
--- a/scripts/add/amplifier_c3xl.py
+++ b/scripts/add/amplifier_c3xl.py
@@ -0,0 +1,156 @@
+#!/usr/bin/env python3
+import time
+from smbus2 import SMBus
+from collections import namedtuple
+
+# https://datasheets.maximintegrated.com/en/ds/MAX98089.pdf
+
+AmpConfig = namedtuple('AmpConfig', ['name', 'value', 'register', 'offset', 'mask'])
+EQParams = namedtuple('EQParams', ['K', 'k1', 'k2', 'c1', 'c2'])
+
+def configs_from_eq_params(base, eq_params):
+  return [
+    AmpConfig("K (high)", (eq_params.K >> 8), base, 0, 0xFF),
+    AmpConfig("K (low)", (eq_params.K & 0xFF), base + 1, 0, 0xFF),
+    AmpConfig("k1 (high)", (eq_params.k1 >> 8), base + 2, 0, 0xFF),
+    AmpConfig("k1 (low)", (eq_params.k1 & 0xFF), base + 3, 0, 0xFF),
+    AmpConfig("k2 (high)", (eq_params.k2 >> 8), base + 4, 0, 0xFF),
+    AmpConfig("k2 (low)", (eq_params.k2 & 0xFF), base + 5, 0, 0xFF),
+    AmpConfig("c1 (high)", (eq_params.c1 >> 8), base + 6, 0, 0xFF),
+    AmpConfig("c1 (low)", (eq_params.c1 & 0xFF), base + 7, 0, 0xFF),
+    AmpConfig("c2 (high)", (eq_params.c2 >> 8), base + 8, 0, 0xFF),
+    AmpConfig("c2 (low)", (eq_params.c2 & 0xFF), base + 9, 0, 0xFF),
+  ]
+
+BASE_CONFIG = [
+  AmpConfig("MCLK prescaler", 0b01, 0x10, 4, 0b00110000),
+  AmpConfig("PM: enable speakers", 0b11, 0x4D, 4, 0b00110000),
+  AmpConfig("PM: enable DACs", 0b11, 0x4D, 0, 0b00000011),
+  AmpConfig("Enable PLL1", 0b1, 0x12, 7, 0b10000000),
+  AmpConfig("Enable PLL2", 0b1, 0x1A, 7, 0b10000000),
+  AmpConfig("DAI1: I2S mode", 0b00100, 0x14, 2, 0b01111100),
+  AmpConfig("DAI2: I2S mode", 0b00100, 0x1C, 2, 0b01111100),
+  AmpConfig("DAI1 Passband filtering: music mode", 0b1, 0x18, 7, 0b10000000),
+  AmpConfig("DAI1 voice mode gain (DV1G)", 0b00, 0x2F, 4, 0b00110000),
+  AmpConfig("DAI1 attenuation (DV1)", 0x0, 0x2F, 0, 0b00001111),
+  AmpConfig("DAI2 attenuation (DV2)", 0x0, 0x31, 0, 0b00001111),
+  AmpConfig("DAI2: DC blocking", 0b1, 0x20, 0, 0b00000001),
+  AmpConfig("DAI2: High sample rate", 0b0, 0x20, 3, 0b00001000),
+  AmpConfig("ALC enable", 0b1, 0x43, 7, 0b10000000),
+  AmpConfig("ALC/excursion limiter release time", 0b101, 0x43, 4, 0b01110000),
+  AmpConfig("ALC multiband enable", 0b1, 0x43, 3, 0b00001000),
+  AmpConfig("DAI1 EQ enable", 0b0, 0x49, 0, 0b00000001),
+  AmpConfig("DAI2 EQ clip detection disabled", 0b1, 0x32, 4, 0b00010000),
+  AmpConfig("DAI2 EQ attenuation", 0x5, 0x32, 0, 0b00001111),
+  AmpConfig("Excursion limiter upper corner freq", 0b100, 0x41, 4, 0b01110000),
+  AmpConfig("Excursion limiter lower corner freq", 0b00, 0x41, 0, 0b00000011),
+  AmpConfig("Excursion limiter threshold", 0b000, 0x42, 0, 0b00001111),
+  AmpConfig("Distortion limit (THDCLP)", 0x6, 0x46, 4, 0b11110000),
+  AmpConfig("Distortion limiter release time constant", 0b0, 0x46, 0, 0b00000001),
+  AmpConfig("Right DAC input mixer: DAI1 left", 0b0, 0x22, 3, 0b00001000),
+  AmpConfig("Right DAC input mixer: DAI1 right", 0b0, 0x22, 2, 0b00000100),
+  AmpConfig("Right DAC input mixer: DAI2 left", 0b1, 0x22, 1, 0b00000010),
+  AmpConfig("Right DAC input mixer: DAI2 right", 0b0, 0x22, 0, 0b00000001),
+  AmpConfig("DAI1 audio port selector", 0b10, 0x16, 6, 0b11000000),
+  AmpConfig("DAI2 audio port selector", 0b01, 0x1E, 6, 0b11000000),
+  AmpConfig("Enable left digital microphone", 0b1, 0x48, 5, 0b00100000),
+  AmpConfig("Enable right digital microphone", 0b1, 0x48, 4, 0b00010000),
+  AmpConfig("Enhanced volume smoothing disabled", 0b0, 0x49, 7, 0b10000000),
+  AmpConfig("Volume adjustment smoothing disabled", 0b0, 0x49, 6, 0b01000000),
+  AmpConfig("Zero-crossing detection disabled", 0b0, 0x49, 5, 0b00100000),
+]
+
+CONFIGS = {
+  "tici": [
+    AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111),
+    AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100),
+    AmpConfig("Right speaker output volume", 0x1c, 0x3E, 0, 0b00011111),
+    AmpConfig("DAI2 EQ enable", 0b1, 0x49, 1, 0b00000010),
+
+    *configs_from_eq_params(0x84, EQParams(0x274F, 0xC0FF, 0x3BF9, 0x0B3C, 0x1656)),
+    *configs_from_eq_params(0x8E, EQParams(0x1009, 0xC6BF, 0x2952, 0x1C97, 0x30DF)),
+    *configs_from_eq_params(0x98, EQParams(0x0F75, 0xCBE5, 0x0ED2, 0x2528, 0x3E42)),
+    *configs_from_eq_params(0xA2, EQParams(0x091F, 0x3D4C, 0xCE11, 0x1266, 0x2807)),
+    *configs_from_eq_params(0xAC, EQParams(0x0A9E, 0x3F20, 0xE573, 0x0A8B, 0x3A3B)),
+  ],
+  "tizi": [
+    AmpConfig("Left speaker output from left DAC", 0b1, 0x2B, 0, 0b11111111),
+    AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111),
+    AmpConfig("Left Speaker Mixer Gain", 0b00, 0x2D, 0, 0b00000011),
+    AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100),
+    AmpConfig("Left speaker output volume", 0x17, 0x3D, 0, 0b00011111),
+    AmpConfig("Right speaker output volume", 0x17, 0x3E, 0, 0b00011111),
+
+    AmpConfig("DAI2 EQ enable", 0b0, 0x49, 1, 0b00000010),
+    AmpConfig("DAI2: DC blocking", 0b0, 0x20, 0, 0b00000001),
+    AmpConfig("ALC enable", 0b0, 0x43, 7, 0b10000000),
+    AmpConfig("DAI2 EQ attenuation", 0x2, 0x32, 0, 0b00001111),
+    AmpConfig("Excursion limiter upper corner freq", 0b001, 0x41, 4, 0b01110000),
+    AmpConfig("Excursion limiter threshold", 0b100, 0x42, 0, 0b00001111),
+    AmpConfig("Distortion limit (THDCLP)", 0x0, 0x46, 4, 0b11110000),
+    AmpConfig("Distortion limiter release time constant", 0b1, 0x46, 0, 0b00000001),
+    AmpConfig("Left DAC input mixer: DAI1 left", 0b0, 0x22, 7, 0b10000000),
+    AmpConfig("Left DAC input mixer: DAI1 right", 0b0, 0x22, 6, 0b01000000),
+    AmpConfig("Left DAC input mixer: DAI2 left", 0b1, 0x22, 5, 0b00100000),
+    AmpConfig("Left DAC input mixer: DAI2 right", 0b0, 0x22, 4, 0b00010000),
+    AmpConfig("Right DAC input mixer: DAI2 left", 0b0, 0x22, 1, 0b00000010),
+    AmpConfig("Right DAC input mixer: DAI2 right", 0b1, 0x22, 0, 0b00000001),
+    AmpConfig("Volume adjustment smoothing disabled", 0b1, 0x49, 6, 0b01000000),
+  ],
+}
+
+class Amplifier:
+  AMP_I2C_BUS = 0
+  AMP_ADDRESS = 0x10
+
+  def __init__(self, debug=False):
+    self.debug = debug
+
+  def _get_shutdown_config(self, amp_disabled: bool) -> AmpConfig:
+    return AmpConfig("Global shutdown", 0b0 if amp_disabled else 0b1, 0x51, 7, 0b10000000)
+
+  def _set_configs(self, configs: list[AmpConfig]) -> None:
+    with SMBus(self.AMP_I2C_BUS) as bus:
+      for config in configs:
+        if self.debug:
+          print(f"Setting \"{config.name}\" to {config.value}:")
+
+        old_value = bus.read_byte_data(self.AMP_ADDRESS, config.register, force=True)
+        new_value = (old_value & (~config.mask)) | ((config.value << config.offset) & config.mask)
+        bus.write_byte_data(self.AMP_ADDRESS, config.register, new_value, force=True)
+
+        if self.debug:
+          print(f"  Changed {hex(config.register)}: {hex(old_value)} -> {hex(new_value)}")
+
+  def set_configs(self, configs: list[AmpConfig]) -> bool:
+    # retry in case panda is using the amp
+    tries = 1
+    for i in range(1):
+      try:
+        self._set_configs(configs)
+        return True
+      except OSError:
+        print(f"Failed to set amp config, {tries - i - 1} retries left")
+        time.sleep(0.02)
+    return False
+
+  def set_global_shutdown(self, amp_disabled: bool) -> bool:
+    return self.set_configs([self._get_shutdown_config(amp_disabled), ])
+
+  def initialize_configuration(self, model: str) -> bool:
+    cfgs = [
+      self._get_shutdown_config(True),
+      *BASE_CONFIG,
+      *CONFIGS[model],
+      self._get_shutdown_config(False),
+    ]
+    return self.set_configs(cfgs)
+
+
+#if __name__ == "__main__":
+  #with open("/sys/firmware/devicetree/base/model") as f:
+   # model = f.read().strip('\x00')
+  #model = model.split('comma ')[-1]
+
+  #amp = Amplifier()
+  #amp.initialize_configuration(model)
--- a/selfdrive/car/cruise.py
+++ b/selfdrive/car/cruise.py
@@ -342,7 +342,7 @@ class VCruiseCarrot:
          self.v_cruise_kph = np.clip(v_cruise_kph, 30, self._cruise_speed_max)
          self.v_cruise_cluster_kph = self.v_cruise_kph
    else:
-      self.v_cruise_kph = max(20, self.v_ego_kph_set) #V_CRUISE_UNSET
+      self.v_cruise_kph = np.clip(v_cruise_kph, self._cruise_speed_min, self._cruise_speed_max) #max(20, self.v_ego_kph_set) #V_CRUISE_UNSET
      self.v_cruise_cluster_kph = self.v_cruise_kph #V_CRUISE_UNSET
      #if self.cruise_state_available_last: # 최초 한번이라도 cruiseState.available이 True였다면
      #  self._lat_enabled = False
@@ -495,7 +495,7 @@ class VCruiseCarrot:
        if self._soft_hold_active > 0:
          self._soft_hold_active = 0
        elif self._cruise_ready or not CC.enabled or CS.cruiseState.standstill or self.carrot_cruise_active:
-          if self._cruise_button_mode in [2, 3]:
+          if False: #self._cruise_button_mode in [2, 3]:
            road_limit_kph = self.nRoadLimitSpeed * self.autoSpeedUptoRoadSpeedLimit
            if road_limit_kph > 1.0:
              v_cruise_kph = max(v_cruise_kph, road_limit_kph)
@@ -636,7 +636,7 @@ class VCruiseCarrot:
        v_cruise_kph = self.nRoadLimitSpeed + self.autoRoadSpeedLimitOffset
    elif self.nRoadLimitSpeed < self.nRoadLimitSpeed_last and self.autoRoadSpeedAdjust > 0:
      new_road_limit_kph = self.nRoadLimitSpeed * self.autoRoadSpeedAdjust + v_cruise_kph * (1 - self.autoRoadSpeedAdjust)
-      self._add_log(f"AutoSpeed change {v_cruise_kph} -> {new_road_limit_kph}")
+      self._add_log(f"AutoSpeed change {v_cruise_kph} -> {new_road_limit_kph:.1f}")
      v_cruise_kph = min(v_cruise_kph, new_road_limit_kph)
    self.road_limit_kph = road_limit_kph
    self.nRoadLimitSpeed_last = self.nRoadLimitSpeed
--- a/selfdrive/carrot/carrot_functions.py
+++ b/selfdrive/carrot/carrot_functions.py
@@ -79,7 +79,7 @@ class CarrotPlanner:
    self.stopSignCount = 0

    self.stop_distance = 6.0
-    self.trafficStopDistanceAdjust = 2.0 #params.get_float("TrafficStopDistanceAdjust") / 100.
+    self.trafficStopDistanceAdjust = 2.5 #params.get_float("TrafficStopDistanceAdjust") / 100.
    self.comfortBrake = 2.4
    self.comfort_brake = self.comfortBrake

@@ -110,6 +110,8 @@ class CarrotPlanner:
    self.cruiseMaxVals5 = 0.7
    self.cruiseMaxVals6 = 0.6

+    self.aChangeCostStarting = 10.0
+
    self.trafficLightDetectMode = 2 # 0: None, 1:Stop, 2:Stop&Go
    self.trafficState_carrot = 0
    self.carrot_stay_stop = False
@@ -170,7 +172,8 @@ class CarrotPlanner:
      self.j_lead_factor = self.params.get_float("JLeadFactor3") / 100.
      self.eco_over_speed = self.params.get_int("CruiseEcoControl")
      self.autoNaviSpeedDecelRate = float(self.params.get_int("AutoNaviSpeedDecelRate")) * 0.01
-
+      self.aChangeCostStaring = self.params.get_float("AChangeCostStarting")
+      self.trafficStopDistanceAdjust = self.params.get_float("TrafficStopDistanceAdjust") / 100.
    elif self.params_count >= 100:

      self.params_count = 0
@@ -213,7 +216,7 @@ class CarrotPlanner:
      t_follow *= self.dynamicTFollowLC   # 차선변경시 t_follow를 줄임.
      self.jerk_factor_apply = self.jerk_factor * self.dynamicTFollowLC   # 차선변경시 jerk factor를 줄여 aggresive하게
    elif lead.status:
-      t_follow += np.interp(prev_a[0], [-2.0, -0.5], [0.2, 0.0])
+      t_follow += np.interp(prev_a[0], [-2.0, -0.5], [0.1, 0.0])
      if self.dynamicTFollow > 0.0:
        gap_dist_adjust = np.clip((desired_follow_distance - lead.dRel) * self.dynamicTFollow, - 0.1, 1.0) * 0.1
        t_follow += gap_dist_adjust
@@ -350,9 +353,9 @@ class CarrotPlanner:

    leadOne = radarstate.leadOne
    self.mySafeFactor = 1.0
-    if leadOne.status and leadOne.vLead < 5:
-      self.mySafeFactor = self.mySafeModeFactor
-    elif self.myDrivingMode == DrivingMode.Eco: # eco
+    if leadOne.status and leadOne.vLead < 5 and leadOne.aLead < 0.2 and v_ego > 1.0: # 앞차가 매우 느리거나 정지한경우
+      self.myDrivingMode = DrivingMode.Safe
+    if self.myDrivingMode == DrivingMode.Eco: # eco
      self.mySafeFactor = self.myEcoModeFactor
    elif self.myDrivingMode == DrivingMode.Safe: #safe
      self.mySafeFactor = self.mySafeModeFactor
--- a/selfdrive/carrot/carrot_serv.py
+++ b/selfdrive/carrot/carrot_serv.py
@@ -979,10 +979,15 @@ class CarrotServ:
    if self.turnSpeedControlMode == 2:
      if -500 < self.xDistToTurn < 500:
        speed_n_sources.append((route_speed, "route"))
-    elif self.turnSpeedControlMode == 3:
+    elif self.turnSpeedControlMode in [3, 4]:
      speed_n_sources.append((route_speed, "route"))
      #speed_n_sources.append((self.calculate_current_speed(dist, speed * self.mapTurnSpeedFactor, 0, 1.2), "route"))

+
+    model_turn_speed = max(sm['modelV2'].meta.modelTurnSpeed, self.autoCurveSpeedLowerLimit)
+    if model_turn_speed < 200 and abs(vturn_speed) < 120:
+      speed_n_sources.append((model_turn_speed, "model"))
+
    desired_speed, source = min(speed_n_sources, key=lambda x: x[0])

    if CS is not None:
--- a/selfdrive/carrot_settings.json
+++ b/selfdrive/carrot_settings.json
@@ -261,6 +261,19 @@
      "default": 30,
      "unit": 1
    },
+    {
+      "group": "조향튜닝",
+      "name": "LatSmoothSec",
+      "title": "LatSmoothSec(13)x0.01",
+      "descr": "조향필터링값\n 높으면 부드러운조향",
+      "egroup": "LAT",
+      "etitle": "LatSmoothSec(30)",
+      "edescr": "Lat smoothing values — Higher values smoother",
+      "min": 1,
+      "max": 30,
+      "default": 13,
+      "unit": 1
+    },
    {
      "group": "크루즈",
      "name": "CruiseOnDist",
@@ -447,15 +460,41 @@
      "group": "주행튜닝",
      "name": "StopDistanceCarrot",
      "title": "StopDistance(600)cm",
-      "descr": "신호등 감지 시 정지위치",
+      "descr": "정지위치 * 0.8",
      "egroup": "LONG",
      "etitle": "StopDistance(600)cm",
-      "edescr": "Stop Position When Traffic Light Detected",
+      "edescr": "Stop Distance * 0.8",
      "min": 400,
      "max": 1000,
      "default": 600,
      "unit": 10
    },
+    {
+      "group": "주행튜닝",
+      "name": "AChangeCostStarting",
+      "title": "AChangeCostStarting",
+      "descr": "출발가속도제한, 0:빠른가속도",
+      "egroup": "LONG",
+      "etitle": "AChangeCostStarting",
+      "edescr": "AccelCost Starting, 0:rapid starting",
+      "min": 0,
+      "max": 200,
+      "default": 10,
+      "unit": 10
+    },
+    {
+      "group": "주행튜닝",
+      "name": "TrafficStopDistanceAdjust",
+      "title": "TrafficStopDistanceAdjust(400)",
+      "descr": "신호정지위치 보정",
+      "egroup": "LONG",
+      "etitle": "TrafficStopDistanceAdjust",
+      "edescr": "",
+      "min": -600,
+      "max": 600,
+      "default": 400,
+      "unit": 100
+    },
    {
      "group": "주행튜닝",
      "name": "JLeadFactor3",
@@ -1718,6 +1757,19 @@
      "default": 100,
      "unit": 10
    },
+    {
+      "group": "감속제어",
+      "name": "ModelTurnSpeedFactor",
+      "title": "모델턴속도반영시간(0)x0.1sec",
+      "descr": "클수록 모델의 미래속도가 반영됩니다.",
+      "egroup": "SPEED",
+      "etitle": "ModelTurnSpeed time(0)x0.1sec",
+      "edescr": "future model speed",
+      "min": 0,
+      "max": 80,
+      "default": 0,
+      "unit": 10
+    },
    {
      "group": "감속제어",
      "name": "AutoNaviSpeedSafetyFactor",
--- a/selfdrive/controls/beep.py
+++ b/selfdrive/controls/beep.py
@@ -0,0 +1,139 @@
+#!/usr/bin/env python3
+import subprocess
+import time
+from cereal import car, messaging
+from openpilot.common.realtime import Ratekeeper
+import threading
+
+AudibleAlert = car.CarControl.HUDControl.AudibleAlert
+
+class Beepd:
+  def __init__(self):
+    self.current_alert = AudibleAlert.none
+    self.enable_gpio()
+    self.startup_beep()
+
+  def enable_gpio(self):
+    # 尝试 export，忽略已 export 的错误
+    try:
+      subprocess.run("echo 42 | sudo tee /sys/class/gpio/export",
+                     shell=True,
+                     stderr=subprocess.DEVNULL,
+                     stdout=subprocess.DEVNULL,
+                     encoding='utf8')
+    except Exception:
+      pass
+    subprocess.run("echo \"out\" | sudo tee /sys/class/gpio/gpio42/direction",
+                   shell=True,
+                   stderr=subprocess.DEVNULL,
+                   stdout=subprocess.DEVNULL,
+                   encoding='utf8')
+
+  def _beep(self, on):
+    val = "1" if on else "0"
+    subprocess.run(f"echo \"{val}\" | sudo tee /sys/class/gpio/gpio42/value",
+                   shell=True,
+                   stderr=subprocess.DEVNULL,
+                   stdout=subprocess.DEVNULL,
+                   encoding='utf8')
+
+  def engage(self):
+    self._beep(True)
+    time.sleep(0.05)
+    self._beep(False)
+
+  def disengage(self):
+    for _ in range(2):
+      self._beep(True)
+      time.sleep(0.01)
+      self._beep(False)
+      time.sleep(0.01)
+
+  def warning(self):
+    for _ in range(3):
+      self._beep(True)
+      time.sleep(0.01)
+      self._beep(False)
+      time.sleep(0.01)
+
+  def startup_beep(self):
+    self._beep(True)
+    time.sleep(0.1)
+    self._beep(False)
+
+  def ding(self):
+    self._beep(True)
+    time.sleep(0.02)
+    self._beep(False)
+
+  def dong(self):
+    self._beep(True)
+    time.sleep(0.03)
+    self._beep(False)
+
+  def beep(self):
+    self._beep(True)
+    time.sleep(0.04)
+    self._beep(False)
+
+  def dispatch_beep(self, func):
+    threading.Thread(target=func, daemon=True).start()
+
+  def update_alert(self, new_alert):
+    if new_alert != self.current_alert:
+      self.current_alert = new_alert
+      print(f"[BEEP] New alert: {new_alert}")
+      if new_alert == AudibleAlert.engage:
+        self.dispatch_beep(self.engage)
+      elif new_alert == AudibleAlert.disengage:
+        self.dispatch_beep(self.disengage)
+      elif new_alert in [AudibleAlert.refuse, AudibleAlert.prompt, AudibleAlert.warningImmediate,AudibleAlert.warningSoft]:
+        self.dispatch_beep(self.warning)
+      elif new_alert in [AudibleAlert.longEngaged, AudibleAlert.longDisengaged, AudibleAlert.trafficSignGreen, AudibleAlert.trafficSignChanged, AudibleAlert.trafficError, AudibleAlert.bsdWarning, AudibleAlert.laneChange]:
+        self.dispatch_beep(self.ding)
+      elif new_alert in [AudibleAlert.stopStop, AudibleAlert.stopping, AudibleAlert.autoHold, AudibleAlert.engage2, AudibleAlert.disengage2, AudibleAlert.speedDown, AudibleAlert.audioTurn, AudibleAlert.reverseGear]:
+        self.dispatch_beep(self.dong)
+      elif new_alert in [AudibleAlert.audio1, AudibleAlert.audio2, AudibleAlert.audio3, AudibleAlert.audio4, AudibleAlert.audio5,
+                         AudibleAlert.audio6, AudibleAlert.audio7, AudibleAlert.audio8, AudibleAlert.audio9, AudibleAlert.audio10]:
+        self.dispatch_beep(self.beep)
+
+  def get_audible_alert(self, sm):
+    if sm.updated['selfdriveState']:
+      new_alert = sm['selfdriveState'].alertSound.raw
+      self.update_alert(new_alert)
+
+  def test_beepd_thread(self):
+    frame = 0
+    rk = Ratekeeper(20)
+    pm = messaging.PubMaster(['selfdriveState'])
+    while True:
+      cs = messaging.new_message('selfdriveState')
+      if frame == 40:
+        cs.selfdriveState.alertSound = AudibleAlert.engage
+      if frame == 60:
+        cs.selfdriveState.alertSound = AudibleAlert.disengage
+      if frame == 80:
+        cs.selfdriveState.alertSound = AudibleAlert.prompt
+
+      pm.send("selfdriveState", cs)
+      frame += 1
+      rk.keep_time()
+
+  def beepd_thread(self, test=False):
+    if test:
+      threading.Thread(target=self.test_beepd_thread, daemon=True).start()
+
+    sm = messaging.SubMaster(['selfdriveState'])
+    rk = Ratekeeper(20)
+
+    while True:
+      sm.update(0)
+      self.get_audible_alert(sm)
+      rk.keep_time()
+
+def main():
+  s = Beepd()
+  s.beepd_thread(test=False)  # 改成 True 可启用模拟测试数据
+
+if __name__ == "__main__":
+  main()
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -26,6 +26,7 @@ from openpilot.common.realtime import DT_CTRL, DT_MDL
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
 from selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
+from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose

 State = log.SelfdriveState.OpenpilotState
 LaneChangeState = log.LaneChangeState
@@ -45,15 +46,19 @@ class Controls:

    self.disable_dm = False

-    self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
-                                   'liveCalibration', 'liveLocationKalman', 'longitudinalPlan', 'carState', 'carOutput',
-                                   'liveDelay', 'carrotMan', 'lateralPlan', 'radarState',
+    self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
+                                   'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
+                                   'carrotMan', 'lateralPlan', 'radarState', 'liveLocationKalman', 
                                   'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
    self.pm = messaging.PubMaster(['carControl', 'controlsState'])

    self.steer_limited_by_controls = False
    self.curvature = 0.0
    self.desired_curvature = 0.0
+
+    self.pose_calibrator = PoseCalibrator()
+    self.calibrated_pose: Pose | None = None
+    
    self.yStd = 0.0

    self.side_state = {
@@ -73,6 +78,11 @@ class Controls:

  def update(self):
    self.sm.update(15)
+    if self.sm.updated["liveCalibration"]:
+      self.pose_calibrator.feed_live_calib(self.sm['liveCalibration'])
+    if self.sm.updated["livePose"]:
+      device_pose = Pose.from_live_pose(self.sm['livePose'])
+      self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)

  def state_control(self):
    CS = self.sm['carState']
@@ -138,7 +148,7 @@ class Controls:
    lat_plan = self.sm['lateralPlan']
    curve_speed_abs = abs(self.sm['carrotMan'].vTurnSpeed)
    self.lanefull_mode_enabled = (lat_plan.useLaneLines and curve_speed_abs > self.params.get_int("UseLaneLineCurveSpeed"))
-    lat_smooth_seconds = LAT_SMOOTH_SECONDS #self.params.get_float("SteerSmoothSec") * 0.01
+    lat_smooth_seconds = self.params.get_float("LatSmoothSec") * 0.01
    steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01
    if steer_actuator_delay == 0.0:
      steer_actuator_delay = self.sm['liveDelay'].lateralDelay 
@@ -227,6 +237,10 @@ class Controls:

    # Orientation and angle rates can be useful for carcontroller
    # Only calibrated (car) frame is relevant for the carcontroller
+    #if self.calibrated_pose is not None:
+    #  CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist()
+    #  CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist()
+
    orientation_value = list(self.sm['liveLocationKalman'].calibratedOrientationNED.value)
    if len(orientation_value) > 2:
      CC.orientationNED = orientation_value
--- a/selfdrive/controls/lib/desire_helper.py
+++ b/selfdrive/controls/lib/desire_helper.py
@@ -133,6 +133,7 @@ class DesireHelper:
    self.object_detected_count = 0
    self.lane_available_trigger = False
    self.lane_appeared = False
+    self.lane_line_info = 0

    self.laneChangeNeedTorque = 0
    self.laneChangeBsd = 0
@@ -152,6 +153,9 @@ class DesireHelper:
    self.auto_lane_change_enable = False
    self.next_lane_change = False

+    self.modelTurnSpeedFactor = 0.0
+    self.model_turn_speed = 0.0
+
  def check_lane_state(self, modeldata):
    lane_width_left, self.distance_to_road_edge_left, self.distance_to_road_edge_left_far, lane_prob_left = calculate_lane_width(modeldata.laneLines[0], modeldata.laneLineProbs[0],
                                                                                                 modeldata.laneLines[1], modeldata.roadEdges[0])
@@ -192,14 +196,25 @@ class DesireHelper:
      self.turn_disable_count = max(0, self.turn_disable_count - 1)
    #print(f"desire_state = {desire_state}, turn_desire_state = {self.turn_desire_state}, disable_count = {self.desire_disable_count}")

+  def make_model_turn_speed(self, modeldata):
+    if self.modelTurnSpeedFactor > 0:
+      model_turn_speed = np.interp(self.modelTurnSpeedFactor, modeldata.velocity.t, modeldata.velocity.x) * CV.MS_TO_KPH * 1.2
+      self.model_turn_speed = self.model_turn_speed * 0.9 + model_turn_speed * 0.1
+    else:
+      self.model_turn_speed = 200.0
+    
  def update(self, carstate, modeldata, lateral_active, lane_change_prob, carrotMan, radarState):

    if self.frame % 100 == 0:
      self.laneChangeNeedTorque = self.params.get_int("LaneChangeNeedTorque")
      self.laneChangeBsd = self.params.get_int("LaneChangeBsd")
      self.laneChangeDelay = self.params.get_float("LaneChangeDelay") * 0.1
+      self.modelTurnSpeedFactor= self.params.get_float("ModelTurnSpeedFactor") * 0.1
+
    self.frame += 1

+    self.make_model_turn_speed(modeldata)
+
    self.carrot_lane_change_count = max(0, self.carrot_lane_change_count - 1)
    self.lane_change_delay = max(0, self.lane_change_delay - DT_MDL)

@@ -268,7 +283,8 @@ class DesireHelper:
    blinker_state = driver_blinker_state if driver_desire_enabled else atc_blinker_state

    lane_line_info = carstate.leftLaneLine if blinker_state == BLINKER_LEFT else carstate.rightLaneLine
-    
+
+    lane_line_info_edge_detect = False
    if desire_enabled:
      lane_exist_counter = self.lane_exist_left_count.counter if blinker_state == BLINKER_LEFT else self.lane_exist_right_count.counter
      lane_available = self.available_left_lane if blinker_state == BLINKER_LEFT else self.available_right_lane
@@ -279,7 +295,9 @@ class DesireHelper:
      side_object_dist = radar.dRel + radar.vLead * 4.0 if radar.status else 255
      object_detected = side_object_dist < v_ego * 3.0
      self.object_detected_count = max(1, self.object_detected_count + 1) if object_detected else min(-1, self.object_detected_count - 1)
-
+      if lane_line_info % 10 in [0, 5] and self.lane_line_info not in [0, 5]:
+        lane_line_info_edge_detect = True
+      self.lane_line_info = lane_line_info % 10
    else:
      lane_exist_counter = 0
      lane_available = True
@@ -287,6 +305,7 @@ class DesireHelper:
      self.lane_appeared = False
      self.lane_available_trigger = False
      self.object_detected_count = 0
+      self.lane_line_info = lane_line_info % 10

    #lane_available_trigger = not self.lane_available_last and lane_available
    lane_change_available = (lane_available or edge_available) and lane_line_info < 20 # lane_line_info가 20보다 작으면 흰색라인임.
@@ -371,7 +390,7 @@ class DesireHelper:
          self.lane_change_state = LaneChangeState.off
          self.lane_change_direction = LaneChangeDirection.none
        else:
-          if lane_change_available and self.lane_change_delay == 0:
+          if (lane_change_available and self.lane_change_delay == 0) or lane_line_info_edge_detect:
            if self.blindspot_detected_counter > 0 and not ignore_bsd:  # BSD검출시
              if torque_applied and not block_lanechange_bsd:
                self.lane_change_state = LaneChangeState.laneChangeStarting
@@ -382,7 +401,7 @@ class DesireHelper:
              self.lane_change_state = LaneChangeState.laneChangeStarting
            # ATC작동인경우 차선이 나타나거나 차선이 생기면 차선변경 시작
            # lane_appeared: 차선이 생기는건 안함.. 위험.
-            elif torque_applied or auto_lane_change_trigger:
+            elif torque_applied or auto_lane_change_trigger or lane_line_info_edge_detect:
              self.lane_change_state = LaneChangeState.laneChangeStarting

      # LaneChangeState.laneChangeStarting
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -66,6 +66,10 @@ class LongControl:
    self.stopping_accel = 0
    self.j_lead = 0.0

+    self.use_accel_pid = False
+    if CP.brand == "toyota":
+      self.use_accel_pid = True
+
  def reset(self):
    self.pid.reset()

@@ -121,8 +125,10 @@ class LongControl:
      self.reset()

    else:  # LongCtrlState.pid
-      #error = a_target_now - CS.aEgo
-      error = v_target_now - CS.vEgo
+      if self.use_accel_pid:
+        error = a_target_ff - CS.aEgo
+      else:
+        error = v_target_now - CS.vEgo
      output_accel = self.pid.update(error, speed=CS.vEgo,
                                     feedforward=a_target_ff)

--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -33,13 +33,13 @@ COST_E_DIM = 5
 COST_DIM = COST_E_DIM + 1
 CONSTR_DIM = 4

-X_EGO_OBSTACLE_COST = 3.
+X_EGO_OBSTACLE_COST = 5.
 X_EGO_COST = 0.
 V_EGO_COST = 0.
 A_EGO_COST = 0.
 J_EGO_COST = 5.0
-A_CHANGE_COST = 250.
-A_CHANGE_COST_STARTING = 30.
+A_CHANGE_COST = 200.
+A_CHANGE_COST_STARTING = 10. #30.
 DANGER_ZONE_COST = 100.
 CRASH_DISTANCE = .25
 LEAD_DANGER_FACTOR = 0.8 # 0.75
@@ -294,10 +294,10 @@ class LongitudinalMpc:
    for i in range(N):
      self.solver.cost_set(i, 'Zl', Zl)

-  def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, jerk_factor=1.0):
+  def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, jerk_factor=1.0, a_change_cost_starting=A_CHANGE_COST_STARTING):
    #jerk_factor = get_jerk_factor(personality)
    if self.mode == 'acc':
-      a_change_cost = self.a_change_cost if prev_accel_constraint else A_CHANGE_COST_STARTING
+      a_change_cost = self.a_change_cost if prev_accel_constraint else a_change_cost_starting
      cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
      constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
    elif self.mode == 'blended':
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -171,7 +171,7 @@ class LongitudinalPlanner:
    accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
    accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)

-    self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality, jerk_factor = carrot.jerk_factor_apply)
+    self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality, jerk_factor = carrot.jerk_factor_apply, a_change_cost_starting = carrot.aChangeCostStarting)
    self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
    self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
    self.mpc.update(carrot, reset_state, sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality)
--- a/selfdrive/locationd/SConscript
+++ b/selfdrive/locationd/SConscript
@@ -7,6 +7,13 @@ rednose_gen_dir = 'models/generated'
 rednose_gen_deps = [
  "models/constants.py",
 ]
+pose_ekf = env.RednoseCompileFilter(
+  target='pose',
+  filter_gen_script='models/pose_kf.py',
+  output_dir=rednose_gen_dir,
+  extra_gen_artifacts=[],
+  gen_script_deps=rednose_gen_deps,
+)
 live_ekf = env.RednoseCompileFilter(
  target='live',
  filter_gen_script='models/live_kf.py',
--- a/selfdrive/locationd/lagd.py
+++ b/selfdrive/locationd/lagd.py
@@ -11,7 +11,7 @@ from cereal.services import SERVICE_LIST
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process
 from openpilot.common.swaglog import cloudlog
-from openpilot.selfdrive.locationd.helpers import fft_next_good_size, parabolic_peak_interp
+from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose, fft_next_good_size, parabolic_peak_interp

 BLOCK_SIZE = 100
 BLOCK_NUM = 50
@@ -151,14 +151,13 @@ class BlockAverage:


 class LateralLagEstimator:
-  inputs = {"carControl", "carState", "controlsState", "liveLocationKalman"}
+  inputs = {"carControl", "carState", "controlsState", "liveCalibration", "livePose"}

  def __init__(self, CP: car.CarParams, dt: float,
               block_count: int = BLOCK_NUM, min_valid_block_count: int = BLOCK_NUM_NEEDED, block_size: int = BLOCK_SIZE,
               window_sec: float = MOVING_WINDOW_SEC, okay_window_sec: float = MIN_OKAY_WINDOW_SEC, min_recovery_buffer_sec: float = MIN_RECOVERY_BUFFER_SEC,
               min_vego: float = MIN_VEGO, min_yr: float = MIN_ABS_YAW_RATE, min_ncc: float = MIN_NCC,
-               max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF, min_confidence: float = MIN_CONFIDENCE,
-               enabled: bool = True):
+               max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF, min_confidence: float = MIN_CONFIDENCE):
    self.dt = dt
    self.window_sec = window_sec
    self.okay_window_sec = okay_window_sec
@@ -173,7 +172,6 @@ class LateralLagEstimator:
    self.min_confidence = min_confidence
    self.max_lat_accel = max_lat_accel
    self.max_lat_accel_diff = max_lat_accel_diff
-    self.enabled = enabled

    self.t = 0.0
    self.lat_active = False
@@ -191,7 +189,7 @@ class LateralLagEstimator:
    self.last_pose_invalid_t = 0.0
    self.last_estimate_t = 0.0

-    self.calib_valid = False
+    self.calibrator = PoseCalibrator()

    self.reset(self.initial_lag, 0)

@@ -208,7 +206,7 @@ class LateralLagEstimator:
    liveDelay = msg.liveDelay

    valid_mean_lag, valid_std, current_mean_lag, current_std = self.block_avg.get()
-    if self.enabled and self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std):
+    if self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std):
      if valid_std > MAX_LAG_STD:
        liveDelay.status = log.LiveDelayData.Status.invalid
      else:
@@ -245,11 +243,14 @@ class LateralLagEstimator:
    elif which == "controlsState":
      self.steering_saturated = getattr(msg.lateralControlState, msg.lateralControlState.which()).saturated
      self.desired_curvature = msg.desiredCurvature
-    elif which == 'liveLocationKalman':
-      self.yaw_rate = msg.angularVelocityCalibrated.value[2]
-      self.yaw_rate_std = msg.angularVelocityCalibrated.std[2]    
-      self.pose_valid = msg.angularVelocityCalibrated.valid and msg.posenetOK and msg.inputsOK  
-      self.calib_valid = msg.status == log.LiveLocationKalman.Status.valid
+    elif which == "liveCalibration":
+      self.calibrator.feed_live_calib(msg)
+    elif which == "livePose":
+      device_pose = Pose.from_live_pose(msg)
+      calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
+      self.yaw_rate = calibrated_pose.angular_velocity.yaw
+      self.yaw_rate_std = calibrated_pose.angular_velocity.yaw_std
+      self.pose_valid = msg.angularVelocityDevice.valid and msg.posenetOK and msg.inputsOK
    self.t = t

  def points_enough(self):
@@ -266,7 +267,7 @@ class LateralLagEstimator:
    turning = np.abs(self.yaw_rate) >= self.min_yr
    sensors_valid = self.pose_valid and np.abs(self.yaw_rate) < MAX_YAW_RATE_SANITY_CHECK and self.yaw_rate_std < MAX_YAW_RATE_SANITY_CHECK
    la_valid = np.abs(la_actual_pose) <= self.max_lat_accel and np.abs(la_desired - la_actual_pose) <= self.max_lat_accel_diff
-    calib_valid = self.calib_valid #self.calibrator.calib_valid
+    calib_valid = self.calibrator.calib_valid

    if not self.lat_active:
      self.last_lat_inactive_t = self.t
@@ -363,15 +364,12 @@ def main():
  DEBUG = bool(int(os.getenv("DEBUG", "0")))

  pm = messaging.PubMaster(['liveDelay'])
-  sm = messaging.SubMaster(['liveLocationKalman', 'carState', 'controlsState', 'carControl'], poll='liveLocationKalman')
+  sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', 'controlsState', 'carControl'], poll='livePose')

  params = Params()
  CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)

-  # TODO: remove me, lagd is in shadow mode on release
-  is_release = params.get_bool("IsReleaseBranch")
-
-  lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['liveLocationKalman'].frequency, enabled=True)
+  lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency)
  if (initial_lag_params := retrieve_initial_lag(params, CP)) is not None:
    lag, valid_blocks = initial_lag_params
    lag_learner.reset(lag, valid_blocks)
--- a/selfdrive/locationd/models/car_kf.py
+++ b/selfdrive/locationd/models/car_kf.py
@@ -93,8 +93,9 @@ class CarKalman(KalmanFilter):
    dim_state = CarKalman.initial_x.shape[0]
    name = CarKalman.name

-    # vehicle models comes from The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars
-    # Model used is in 6.15 with formula from 6.198
+    # 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]
@@ -160,19 +161,18 @@ class CarKalman(KalmanFilter):

    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, steer_ratio=15, stiffness_factor=1, angle_offset=0, P_initial=None):
-    dim_state = self.initial_x.shape[0]
-    dim_state_err = self.P_initial.shape[0]
-    x_init = self.initial_x
-    x_init[States.STEER_RATIO] = steer_ratio
-    x_init[States.STIFFNESS] = stiffness_factor
-    x_init[States.ANGLE_OFFSET] = angle_offset
+  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)

-    if P_initial is not None:
-      self.P_initial = P_initial
-    # init filter
-    self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, self.P_initial,
-                              dim_state, dim_state_err, global_vars=self.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__":
--- a/selfdrive/locationd/paramsd.py
+++ b/selfdrive/locationd/paramsd.py
@@ -1,8 +1,10 @@
 #!/usr/bin/env python3
 import os
-import math
-import json
 import numpy as np
+import capnp
+
+import json
+import math

 import cereal.messaging as messaging
 from cereal import car, log
@@ -10,14 +12,14 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, DT_MDL
 from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
 from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR
+from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 from openpilot.common.swaglog import cloudlog

-
 MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL  # Max 20 deg/s
-ROLL_MAX_DELTA = math.radians(20.0) * DT_MDL  # 20deg in 1 second is well within curvature limits
-ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10)
-ROLL_LOWERED_MAX = math.radians(8)
-ROLL_STD_MAX = math.radians(1.5)
+ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL  # 20deg in 1 second is well within curvature limits
+ROLL_MIN, ROLL_MAX = np.radians(-10), np.radians(10)
+ROLL_LOWERED_MAX = np.radians(8)
+ROLL_STD_MAX = np.radians(1.5)
 LATERAL_ACC_SENSOR_THRESHOLD = 4.0
 OFFSET_MAX = 10.0
 OFFSET_LOWERED_MAX = 8.0
@@ -25,54 +27,82 @@ MIN_ACTIVE_SPEED = 1.0
 LOW_ACTIVE_SPEED = 10.0


-class ParamsLearner:
-  def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=None):
-    self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset, P_initial)
+class VehicleParamsLearner:
+  def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None):
+    self.kf = CarKalman(GENERATED_DIR)

-    self.kf.filter.set_global("mass", CP.mass)
-    self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia)
-    self.kf.filter.set_global("center_to_front", CP.centerToFront)
-    self.kf.filter.set_global("center_to_rear", CP.wheelbase - CP.centerToFront)
-    self.kf.filter.set_global("stiffness_front", CP.tireStiffnessFront)
-    self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear)
+    self.x_initial = CarKalman.initial_x.copy()
+    self.x_initial[States.STEER_RATIO] = steer_ratio
+    self.x_initial[States.STIFFNESS] = stiffness_factor
+    self.x_initial[States.ANGLE_OFFSET] = angle_offset
+    self.P_initial = P_initial if P_initial is not None else CarKalman.P_initial

-    self.active = False
+    self.kf.set_globals(
+      mass=CP.mass,
+      rotational_inertia=CP.rotationalInertia,
+      center_to_front=CP.centerToFront,
+      center_to_rear=CP.wheelbase - CP.centerToFront,
+      stiffness_front=CP.tireStiffnessFront,
+      stiffness_rear=CP.tireStiffnessRear
+    )

-    self.speed = 0.0
-    self.yaw_rate = 0.0
-    self.yaw_rate_std = 0.0
-    self.roll = 0.0
-    self.steering_angle = 0.0
+    self.min_sr, self.max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio

-  def handle_log(self, t, which, msg):
-    if which == 'liveLocationKalman':
-      self.yaw_rate = msg.angularVelocityCalibrated.value[2]
-      self.yaw_rate_std = msg.angularVelocityCalibrated.std[2]
+    self.calibrator = PoseCalibrator()

-      localizer_roll = msg.orientationNED.value[0]
-      localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0]
+    self.observed_speed = 0.0
+    self.observed_yaw_rate = 0.0
+    self.observed_roll = 0.0
+
+    self.avg_offset_valid = True
+    self.total_offset_valid = True
+    self.roll_valid = True
+
+    self.reset(None)
+
+  def reset(self, t: float | None):
+    self.kf.init_state(self.x_initial, covs=self.P_initial, filter_time=t)
+
+    self.angle_offset, self.roll, self.active = np.degrees(self.x_initial[States.ANGLE_OFFSET].item()), 0.0, False
+    self.avg_angle_offset = self.angle_offset
+
+  def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader):
+    if which == 'livePose':
+      device_pose = Pose.from_live_pose(msg)
+      calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
+
+      yaw_rate, yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std
+      yaw_rate_valid = msg.angularVelocityDevice.valid
+      yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10  # rad/s
+      yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1  # rad/s
+      if not yaw_rate_valid:
+        # This is done to bound the yaw rate estimate when localizer values are invalid or calibrating
+        yaw_rate, yaw_rate_std = 0.0, np.radians(10.0)
+      self.observed_yaw_rate = yaw_rate
+
+      localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std
+      localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std
      roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK
      if roll_valid:
        roll = localizer_roll
        # Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar?
-        #roll_std = 2 * localizer_roll_std
-        roll_std = 3 * localizer_roll_std
+        roll_std = 2 * localizer_roll_std
      else:
        # This is done to bound the road roll estimate when localizer values are invalid
        roll = 0.0
        roll_std = np.radians(10.0)
-      self.roll = np.clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
+      self.observed_roll = np.clip(roll, self.observed_roll - ROLL_MAX_DELTA, self.observed_roll + ROLL_MAX_DELTA)

      if self.active:
        if msg.posenetOK:
          self.kf.predict_and_observe(t,
                                      ObservationKind.ROAD_FRAME_YAW_RATE,
-                                      np.array([[-self.yaw_rate]]),
-                                      np.array([np.atleast_2d(self.yaw_rate_std**2)]))
+                                      np.array([[-self.observed_yaw_rate]]),
+                                      np.array([np.atleast_2d(yaw_rate_std**2)]))

          self.kf.predict_and_observe(t,
                                      ObservationKind.ROAD_ROLL,
-                                      np.array([[self.roll]]),
+                                      np.array([[self.observed_roll]]),
                                      np.array([np.atleast_2d(roll_std**2)]))
        self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]]))

@@ -84,21 +114,83 @@ class ParamsLearner:
        self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]]))
        self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]]))

-    elif which == 'carState':
-      self.steering_angle = msg.steeringAngleDeg
-      self.speed = msg.vEgo
+    elif which == 'liveCalibration':
+      self.calibrator.feed_live_calib(msg)

-      in_linear_region = abs(self.steering_angle) < 45
-      self.active = self.speed > MIN_ACTIVE_SPEED and in_linear_region
+    elif which == 'carState':
+      steering_angle = msg.steeringAngleDeg
+
+      in_linear_region = abs(steering_angle) < 45
+      self.observed_speed = msg.vEgo
+      self.active = self.observed_speed > MIN_ACTIVE_SPEED and in_linear_region

      if self.active:
-        self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]]))
-        self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.speed]]))
+        self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[np.radians(steering_angle)]]))
+        self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.observed_speed]]))

    if not self.active:
      # Reset time when stopped so uncertainty doesn't grow
-      self.kf.filter.set_filter_time(t)
-      self.kf.filter.reset_rewind()
+      self.kf.filter.set_filter_time(t)  # type: ignore
+      self.kf.filter.reset_rewind()      # type: ignore
+
+  def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder:
+    x = self.kf.x
+    P = np.sqrt(self.kf.P.diagonal())
+    if not np.all(np.isfinite(x)):
+      cloudlog.error("NaN in liveParameters estimate. Resetting to default values")
+      self.reset(self.kf.t)
+      x = self.kf.x
+
+    self.avg_angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item()),
+                                self.avg_angle_offset - MAX_ANGLE_OFFSET_DELTA, self.avg_angle_offset + MAX_ANGLE_OFFSET_DELTA)
+    self.angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
+                        self.angle_offset - MAX_ANGLE_OFFSET_DELTA, self.angle_offset + MAX_ANGLE_OFFSET_DELTA)
+    self.roll = np.clip(float(x[States.ROAD_ROLL].item()), self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
+    roll_std = float(P[States.ROAD_ROLL].item())
+    if self.active and self.observed_speed > LOW_ACTIVE_SPEED:
+      # Account for the opposite signs of the yaw rates
+      # At low speeds, bumping into a curb can cause the yaw rate to be very high
+      sensors_valid = bool(abs(self.observed_speed * (x[States.YAW_RATE].item() + self.observed_yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
+    else:
+      sensors_valid = True
+    self.avg_offset_valid = check_valid_with_hysteresis(self.avg_offset_valid, self.avg_angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
+    self.total_offset_valid = check_valid_with_hysteresis(self.total_offset_valid, self.angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
+    self.roll_valid = check_valid_with_hysteresis(self.roll_valid, self.roll, ROLL_MAX, ROLL_LOWERED_MAX)
+
+    msg = messaging.new_message('liveParameters')
+
+    msg.valid = valid
+
+    liveParameters = msg.liveParameters
+    liveParameters.posenetValid = True
+    liveParameters.sensorValid = sensors_valid
+    liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
+    liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
+    liveParameters.roll = float(self.roll)
+    liveParameters.angleOffsetAverageDeg = float(self.avg_angle_offset)
+    liveParameters.angleOffsetDeg = float(self.angle_offset)
+    liveParameters.steerRatioValid = self.min_sr <= liveParameters.steerRatio <= self.max_sr
+    liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0
+    liveParameters.angleOffsetAverageValid = bool(self.avg_offset_valid)
+    liveParameters.angleOffsetValid = bool(self.total_offset_valid)
+    liveParameters.valid = all((
+      liveParameters.angleOffsetAverageValid,
+      liveParameters.angleOffsetValid ,
+      self.roll_valid,
+      roll_std < ROLL_STD_MAX,
+      liveParameters.stiffnessFactorValid,
+      liveParameters.steerRatioValid,
+    ))
+    liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
+    liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
+    liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
+    liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
+    if debug:
+      liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message()
+      liveParameters.debugFilterState.value = x.tolist()
+      liveParameters.debugFilterState.std = P.tolist()
+
+    return msg


 def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: float, lowered_threshold: float):
@@ -109,6 +201,67 @@ def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: floa
  return current_valid


+# TODO: Remove this function after few releases (added in 0.9.9)
+def migrate_cached_vehicle_params_if_needed(params: Params):
+  last_parameters_data_old = params.get("LiveParameters")
+  last_parameters_data = params.get("LiveParametersV2")
+  if last_parameters_data_old is None or last_parameters_data is not None:
+    return
+
+  try:
+    last_parameters_msg = messaging.new_message('liveParameters')
+    last_parameters_msg.liveParameters.valid = True
+    last_parameters_msg.liveParameters.steerRatio = last_parameters_data_old['steerRatio']
+    last_parameters_msg.liveParameters.stiffnessFactor = last_parameters_data_old['stiffnessFactor']
+    last_parameters_msg.liveParameters.angleOffsetAverageDeg = last_parameters_data_old['angleOffsetAverageDeg']
+    params.put("LiveParametersV2", last_parameters_msg.to_bytes())
+  except Exception as e:
+    cloudlog.error(f"Failed to perform parameter migration: {e}")
+    params.remove("LiveParameters")
+
+
+def retrieve_initial_vehicle_params(params: Params, CP: car.CarParams, replay: bool, debug: bool):
+  last_parameters_data = params.get("LiveParametersV2")
+  last_carparams_data = params.get("CarParamsPrevRoute")
+
+  steer_ratio, stiffness_factor, angle_offset_deg, p_initial = CP.steerRatio, 1.0, 0.0, None
+
+  retrieve_success = False
+  if last_parameters_data is not None and last_carparams_data is not None:
+    try:
+      with log.Event.from_bytes(last_parameters_data) as last_lp_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP:
+        lp = last_lp_msg.liveParameters
+        # Check if car model matches
+        if last_CP.carFingerprint != CP.carFingerprint:
+          raise Exception("Car model mismatch")
+
+        # Check if starting values are sane
+        min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
+        steer_ratio_sane = min_sr <= lp.steerRatio <= max_sr
+        if not steer_ratio_sane:
+          raise Exception(f"Invalid starting values found {lp}")
+
+        initial_filter_std = np.array(lp.debugFilterState.std)
+        if debug and len(initial_filter_std) != 0:
+          p_initial = np.diag(initial_filter_std)
+
+        steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetAverageDeg
+        retrieve_success = True
+    except Exception as e:
+      cloudlog.error(f"Failed to retrieve initial values: {e}")
+      params.remove("LiveParametersV2")
+
+  if not replay:
+    # When driving in wet conditions the stiffness can go down, and then be too low on the next drive
+    # Without a way to detect this we have to reset the stiffness every drive
+    stiffness_factor = 1.0
+
+  if not retrieve_success:
+    cloudlog.info("Parameter learner resetting to default values")
+
+  return steer_ratio, stiffness_factor, angle_offset_deg, p_initial
+
+
 def main():
  config_realtime_process([0, 1, 2, 3], 5)

@@ -116,65 +269,18 @@ def main():
  REPLAY = bool(int(os.getenv("REPLAY", "0")))

  pm = messaging.PubMaster(['liveParameters'])
-  sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll='liveLocationKalman')
+  sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', 'liveLocationKalman'], poll='livePose')

-  params_reader = Params()
-  # wait for stats about the car to come in from controls
-  cloudlog.info("paramsd is waiting for CarParams")
-  CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams)
-  cloudlog.info("paramsd got CarParams")
+  params = Params()
+  CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)

-  min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio
+  migrate_cached_vehicle_params_if_needed(params)

-  params = params_reader.get("LiveParameters")
+  steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params, CP, REPLAY, DEBUG)
+  learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial)

-  # Check if car model matches
-  if params is not None:
-    params = json.loads(params)
-    if params.get('carFingerprint', None) != CP.carFingerprint:
-      cloudlog.info("Parameter learner found parameters for wrong car.")
-      params = None
-
-  # Check if starting values are sane
-  if params is not None:
-    try:
-      steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr
-      if not steer_ratio_sane:
-        cloudlog.info(f"Invalid starting values found {params}")
-        params = None
-    except Exception as e:
-      cloudlog.info(f"Error reading params {params}: {str(e)}")
-      params = None
-
-  # TODO: cache the params with the capnp struct
-  if params is None:
-    params = {
-      'carFingerprint': CP.carFingerprint,
-      'steerRatio': CP.steerRatio,
-      'stiffnessFactor': 1.0,
-      'angleOffsetAverageDeg': 0.0,
-    }
-    cloudlog.info("Parameter learner resetting to default values")
-
-  if not REPLAY:
-    # When driving in wet conditions the stiffness can go down, and then be too low on the next drive
-    # Without a way to detect this we have to reset the stiffness every drive
-    params['stiffnessFactor'] = 1.0
-
-  pInitial = None
-  if DEBUG:
-    pInitial = np.array(params['debugFilterState']['std']) if 'debugFilterState' in params else None
-
-  learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg']), pInitial)
-  angle_offset_average = params['angleOffsetAverageDeg']
-  angle_offset = angle_offset_average
-  roll = 0.0
-  avg_offset_valid = True
-  total_offset_valid = True
-  roll_valid = True
  params_memory = Params("/dev/shm/params")
  params_memory.remove("LastGPSPosition")
-
  while True:
    sm.update()
    if sm.all_checks():
@@ -191,72 +297,15 @@ def main():
        lon = location.positionGeodetic.value[1]
        params_memory.put("LastGPSPosition", json.dumps({"latitude": lat, "longitude": lon, "bearing": bearing}))
        
-      x = learner.kf.x
-      P = np.sqrt(learner.kf.P.diagonal())
-      if not all(map(math.isfinite, x)):
-        cloudlog.error("NaN in liveParameters estimate. Resetting to default values")
-        learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
-        x = learner.kf.x

-      angle_offset_average = np.clip(math.degrees(x[States.ANGLE_OFFSET].item()),
-                                  angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA)
-      angle_offset = np.clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()),
-                          angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
-      roll = np.clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA)
-      roll_std = float(P[States.ROAD_ROLL].item())
-      if learner.active and learner.speed > LOW_ACTIVE_SPEED:
-        # Account for the opposite signs of the yaw rates
-        # At low speeds, bumping into a curb can cause the yaw rate to be very high
-        sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE].item() + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD)
-      else:
-        sensors_valid = True
-      avg_offset_valid = check_valid_with_hysteresis(avg_offset_valid, angle_offset_average, OFFSET_MAX, OFFSET_LOWERED_MAX)
-      total_offset_valid = check_valid_with_hysteresis(total_offset_valid, angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX)
-      roll_valid = check_valid_with_hysteresis(roll_valid, roll, ROLL_MAX, ROLL_LOWERED_MAX)
-
-      msg = messaging.new_message('liveParameters')
-
-      liveParameters = msg.liveParameters
-      liveParameters.posenetValid = True
-      liveParameters.sensorValid = sensors_valid
-      liveParameters.steerRatio = float(x[States.STEER_RATIO].item())
-      liveParameters.steerRatioValid = min_sr <= liveParameters.steerRatio <= max_sr
-      liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item())
-      liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0
-      liveParameters.roll = float(roll)
-      liveParameters.angleOffsetAverageDeg = float(angle_offset_average)
-      liveParameters.angleOffsetAverageValid = bool(avg_offset_valid)
-      liveParameters.angleOffsetDeg = float(angle_offset)
-      liveParameters.angleOffsetValid = bool(total_offset_valid)
-      liveParameters.valid = all((
-        liveParameters.angleOffsetAverageValid,
-        liveParameters.angleOffsetValid ,
-        roll_valid,
-        roll_std < ROLL_STD_MAX,
-        liveParameters.stiffnessFactorValid,
-        liveParameters.steerRatioValid,
-      ))
-      liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item())
-      liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item())
-      liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item())
-      liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item())
-      if DEBUG:
-        liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message()
-        liveParameters.debugFilterState.value = x.tolist()
-        liveParameters.debugFilterState.std = P.tolist()
-
-      msg.valid = sm.all_checks()
+    if sm.updated['livePose']:
+      msg = learner.get_msg(sm.all_checks(), debug=DEBUG)

+      msg_dat = msg.to_bytes()
      if sm.frame % 1200 == 0:  # once a minute
-        params = {
-          'carFingerprint': CP.carFingerprint,
-          'steerRatio': liveParameters.steerRatio,
-          'stiffnessFactor': liveParameters.stiffnessFactor,
-          'angleOffsetAverageDeg': liveParameters.angleOffsetAverageDeg,
-        }
-        params_reader.put_nonblocking("LiveParameters", json.dumps(params))
+        params.put_nonblocking("LiveParametersV2", msg_dat)

-      pm.send('liveParameters', msg)
+      pm.send('liveParameters', msg_dat)


 if __name__ == "__main__":
--- a/selfdrive/locationd/torqued.py
+++ b/selfdrive/locationd/torqued.py
@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+import os
 import numpy as np
 from collections import deque, defaultdict

@@ -9,7 +10,7 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, DT_MDL
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.swaglog import cloudlog
-from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator
+from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose

 HISTORY = 5  # secs
 POINTS_PER_BUCKET = 1500
@@ -32,7 +33,7 @@ MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100])
 MIN_ENGAGE_BUFFER = 2  # secs

 VERSION = 1  # bump this to invalidate old parameter caches
-ALLOWED_CARS = ['toyota', 'hyundai', 'rivian']
+ALLOWED_CARS = ['toyota', 'hyundai', 'rivian', 'honda']


 def slope2rot(slope):
@@ -77,6 +78,8 @@ class TorqueEstimator(ParameterEstimator):
      self.offline_friction = CP.lateralTuning.torque.friction
      self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor

+    self.calibrator = PoseCalibrator()
+
    self.reset()

    initial_params = {
@@ -171,12 +174,19 @@ class TorqueEstimator(ParameterEstimator):
      # TODO: check if high aEgo affects resulting lateral accel
      self.raw_points["vego"].append(msg.vEgo)
      self.raw_points["steer_override"].append(msg.steeringPressed)
+    elif which == "liveCalibration":
+      self.calibrator.feed_live_calib(msg)
    elif which == "liveDelay":
      self.lag = msg.lateralDelay
-    elif which == "liveLocationKalman":
+    # calculate lateral accel from past steering torque
+    elif which == "livePose":
      if len(self.raw_points['steer_torque']) == self.hist_len:
-        yaw_rate = msg.angularVelocityCalibrated.value[2]
-        roll = msg.orientationNED.value[0]
+        device_pose = Pose.from_live_pose(msg)
+        calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
+        angular_velocity_calibrated = calibrated_pose.angular_velocity
+
+        yaw_rate = angular_velocity_calibrated.yaw
+        roll = device_pose.orientation.roll
        # check lat active up to now (without lag compensation)
        lat_active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_MDL),
                               self.raw_points['carControl_t'], self.raw_points['lat_active']).astype(bool)
@@ -234,7 +244,7 @@ def main(demo=False):
  config_realtime_process([0, 1, 2, 3], 5)

  pm = messaging.PubMaster(['liveTorqueParameters'])
-  sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveDelay', 'liveLocationKalman'], poll='liveLocationKalman')
+  sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose', 'liveDelay'], poll='livePose')

  params = Params()
  estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams))
@@ -247,7 +257,7 @@ def main(demo=False):
          t = sm.logMonoTime[which] * 1e-9
          estimator.handle_log(t, which, sm[which])

-    # 4Hz driven by liveLocationKalman
+    # 4Hz driven by livePose
    if sm.frame % 5 == 0:
      pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks()))

--- a/selfdrive/modeld/modeld.py
+++ b/selfdrive/modeld/modeld.py
@@ -311,7 +311,7 @@ def main(demo=False):
    frame += 1
    if frame % 100 == 0:
      custom_lat_delay = params.get_float("SteerActuatorDelay") * 0.01
-      #lat_smooth_seconds = params.get_float("SteerSmoothSec") * 0.01
+      lat_smooth_seconds = params.get_float("LatSmoothSec") * 0.01
      long_delay = params.get_float("LongActuatorDelay")*0.01
      vEgoStopping = params.get_float("VEgoStopping") * 0.01
      
@@ -357,7 +357,7 @@ def main(demo=False):
    is_rhd = sm["driverMonitoringState"].isRHD
    frame_id = sm["roadCameraState"].frameId
    v_ego = max(sm["carState"].vEgo, 0.)
-    #lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
+    #lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32)
    if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
      device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
      dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
@@ -425,6 +425,7 @@ def main(demo=False):
      modelv2_send.modelV2.meta.distanceToRoadEdgeRight = float(DH.distance_to_road_edge_right)
      modelv2_send.modelV2.meta.desire = DH.desire
      modelv2_send.modelV2.meta.laneChangeProb = DH.lane_change_ll_prob
+      modelv2_send.modelV2.meta.modelTurnSpeed = float(DH.model_turn_speed)

      fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen)
      pm.send('modelV2', modelv2_send)
--- a/selfdrive/modeld/models/dmonitoring_model.onnx
+++ b/selfdrive/modeld/models/dmonitoring_model.onnx
--- a/selfdrive/modeld/models/driving_policy.onnx
+++ b/selfdrive/modeld/models/driving_policy.onnx
--- a/selfdrive/selfdrived/selfdrived.py
+++ b/selfdrive/selfdrived/selfdrived.py
@@ -16,6 +16,7 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.common.gps import get_gps_location_service

 from openpilot.selfdrive.car.car_specific import CarSpecificEvents
+from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 from openpilot.selfdrive.selfdrived.events import Events, ET
 from openpilot.selfdrive.selfdrived.state import StateMachine
 from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert
@@ -57,6 +58,8 @@ class SelfdriveD:

    self.car_events = CarSpecificEvents(self.CP)
    self.disengage_on_accelerator = not (self.CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS)
+    self.pose_calibrator = PoseCalibrator()
+    self.calibrated_pose: Pose | None = None

    # Setup sockets
    self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'])
@@ -82,7 +85,7 @@ class SelfdriveD:
      # no vipc in replay will make them ignored anyways
      ignore += ['roadCameraState', 'wideRoadCameraState']
    self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
-                                   'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', # 'liveDelay',
+                                   'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', # 'liveDelay',
                                   'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
                                   'carrotMan',
                                   'controlsState', 'carControl', 'driverAssistance', 'alertDebug'] + \
@@ -239,6 +242,12 @@ class SelfdriveD:
      if self.sm['driverAssistance'].leftLaneDeparture or self.sm['driverAssistance'].rightLaneDeparture:
        self.events.add(EventName.ldw)

+    if self.sm.updated['liveCalibration']:
+      self.pose_calibrator.feed_live_calib(self.sm['liveCalibration'])
+    if self.sm.updated['livePose']:
+      device_pose = Pose.from_live_pose(self.sm['livePose'])
+      self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)
+
    if self.sm.alive['carrotMan']:
      atc_type = self.sm['carrotMan'].atcType
      if atc_type != self.atc_type_last:
@@ -340,11 +349,9 @@ class SelfdriveD:
      self.logged_comm_issue = None

    if not self.CP.notCar:
-      if not self.sm['liveLocationKalman'].posenetOK:
+      if not self.sm['livePose'].posenetOK:
        self.events.add(EventName.posenetInvalid)
-      if not self.sm['liveLocationKalman'].deviceStable:
-        self.events.add(EventName.deviceFalling)
-      if not self.sm['liveLocationKalman'].inputsOK:
+      if not self.sm['livePose'].inputsOK:
        self.events.add(EventName.locationdTemporaryError)
      if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY):
        self.events.add(EventName.paramsdTemporaryError)
@@ -387,7 +394,7 @@ class SelfdriveD:
    if not SIMULATION or REPLAY:
      # Not show in first 1.5 km to allow for driving out of garage. This event shows after 5 minutes
      gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_CTRL < 2.0
-      if not gps_ok and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1500):
+      if not gps_ok and self.sm['livePose'].inputsOK and (self.distance_traveled > 1500):
        if self.distance_traveled < 1600:
          self.events.add(EventName.noGps)
      #if gps_ok:
--- a/selfdrive/ui/qt/offroad/settings.cc
+++ b/selfdrive/ui/qt/offroad/settings.cc
@@ -675,6 +675,8 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
  cruiseToggles->addItem(new CValueControl("MyDrivingMode", tr("DRIVEMODE: Select"), tr("1:ECO,2:SAFE,3:NORMAL,4:HIGH"), 1, 4, 1));
  cruiseToggles->addItem(new CValueControl("MyDrivingModeAuto", tr("DRIVEMODE: Auto"), tr("NORMAL mode only"), 0, 1, 1));
  cruiseToggles->addItem(new CValueControl("TrafficLightDetectMode", tr("TrafficLight DetectMode"), tr("0:None, 1:Stopping only, 2: Stop & Go"), 0, 2, 1));
+  cruiseToggles->addItem(new CValueControl("AChangeCostStarting", tr("AChangeCostStarting"), "", 0, 200, 10));
+  cruiseToggles->addItem(new CValueControl("TrafficStopDistanceAdjust", tr("TrafficStopDistanceAdjust"), "", -600, 600, 50));
  //cruiseToggles->addItem(new CValueControl("CruiseSpeedMin", "CRUISE: Speed Lower limit(10)", "Cruise control MIN speed", 5, 50, 1));
  //cruiseToggles->addItem(new CValueControl("AutoResumeFromGas", "GAS CRUISE ON: Use", "Auto Cruise on when GAS pedal released, 60% Gas Cruise On automatically", 0, 3, 1));
  //cruiseToggles->addItem(new CValueControl("AutoResumeFromGasSpeed", "GAS CRUISE ON: Speed(30)", "Driving speed exceeds the set value, Cruise ON", 20, 140, 5));
@@ -695,6 +697,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
  latLongToggles->addItem(new CValueControl("SteerRatioRate", tr("LAT: SteerRatioRatex0.01(100)"), tr("SteerRatio apply rate"), 30, 170, 1));
  latLongToggles->addItem(new CValueControl("PathOffset", tr("LAT: PathOffset"), tr("(-)left, (+)right"), -150, 150, 1));
  latLongToggles->addItem(new CValueControl("SteerActuatorDelay", tr("LAT:SteerActuatorDelay(30)"), tr("x0.01, 0:LiveDelay"), 0, 100, 1));
+  latLongToggles->addItem(new CValueControl("LatSmoothSec", tr("LAT:LatSmoothSec(13)"), tr("x0.01"), 1, 30, 1));
  latLongToggles->addItem(new CValueControl("LateralTorqueCustom", tr("LAT: TorqueCustom(0)"), "", 0, 2, 1));
  latLongToggles->addItem(new CValueControl("LateralTorqueAccelFactor", tr("LAT: TorqueAccelFactor(2500)"), "", 1000, 6000, 10));
  latLongToggles->addItem(new CValueControl("LateralTorqueFriction", tr("LAT: TorqueFriction(100)"), "", 0, 1000, 10));
@@ -834,6 +837,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
  startToggles->addItem(new CValueControl("HotspotOnBoot", tr("Hotspot enabled on boot"), "", 0, 1, 1));
  startToggles->addItem(new CValueControl("SoftwareMenu", tr("Enable Software Menu"), "", 0, 1, 1));
  startToggles->addItem(new CValueControl("IsLdwsCar", tr("IsLdwsCar"), "", 0, 1, 1));
+  startToggles->addItem(new CValueControl("HardwareC3xLite", tr("Hardware is C3x Lite"), "", 0, 1, 1));

  //startToggles->addItem(new CValueControl("CarrotCountDownSpeed", "NaviCountDown Speed(10)", "", 0, 200, 5));
  //startToggles->addItem(new ParamControl("NoLogging", "Disable Logger", "", this));
@@ -855,6 +859,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
  speedToggles->addItem(new CValueControl("AutoNaviCountDownMode", tr("NaviCountDown mode(2)"), tr("0: off, 1:tbt+camera, 2:tbt+camera+bump"), 0, 2, 1));
  speedToggles->addItem(new CValueControl("TurnSpeedControlMode", tr("Turn Speed control mode(1)"), tr("0: off, 1:vision, 2:vision+route, 3: route"), 0, 3, 1));
  speedToggles->addItem(new CValueControl("MapTurnSpeedFactor", tr("Map TurnSpeed Factor(100)"), "", 50, 300, 5));
+  speedToggles->addItem(new CValueControl("ModelTurnSpeedFactor", tr("Model TurnSpeed Factor(0)"), "", 0, 80, 10));
  speedToggles->addItem(new CValueControl("AutoTurnControl", tr("ATC: Auto turn control(0)"), tr("0:None, 1: lane change, 2: lane change + speed, 3: speed"), 0, 3, 1));
  speedToggles->addItem(new CValueControl("AutoTurnControlSpeedTurn", tr("ATC: Turn Speed (20)"), tr("0:None, turn speed"), 0, 100, 5));
  speedToggles->addItem(new CValueControl("AutoTurnControlTurnEnd", tr("ATC: Turn CtrlDistTime (6)"), tr("dist=speed*time"), 0, 30, 1));
--- a/system/athena/registration.py
+++ b/system/athena/registration.py
@@ -16,6 +16,10 @@ from openpilot.common.swaglog import cloudlog

 UNREGISTERED_DONGLE_ID = "UnregisteredDevice"

+DUMMY_IMEI1 = '865420071781912'
+DUMMY_IMEI2 = '865420071781904'
+
+
 def is_registered_device() -> bool:
  dongle = Params().get("DongleId", encoding='utf-8')
  return dongle not in (None, UNREGISTERED_DONGLE_ID)
@@ -60,6 +64,7 @@ def register(show_spinner=False) -> str | None:
    start_time = time.monotonic()
    imei1: str | None = None
    imei2: str | None = None
+
    while imei1 is None and imei2 is None:
      try:
        imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1)
@@ -67,8 +72,11 @@ def register(show_spinner=False) -> str | None:
        cloudlog.exception("Error getting imei, trying again...")
        time.sleep(1)

-      if time.monotonic() - start_time > 60 and show_spinner:
+      if time.monotonic() - start_time > 30 and show_spinner:
        spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})")
+        imei1 = DUMMY_IMEI1
+        imei2 = DUMMY_IMEI2
+        break

    backoff = 0
    start_time = time.monotonic()
--- a/system/hardware/hardwared.py
+++ b/system/hardware/hardwared.py
@@ -172,7 +172,7 @@ def update_restart_condition(current_time, restart_triggered_ts, params, onroad_
      if softRestartTriggered == 2:
        print("Parameter set to default")
        set_default_params()
-        
+
      params.put_int("SoftRestartTriggered", 0)
      restart_triggered_ts = current_time
  return restart_triggered_ts
@@ -337,20 +337,10 @@ def hardware_thread(end_event, hw_queue) -> None:
    set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", show_alert, extra_text=extra_text)

    # TODO: this should move to TICI.initialize_hardware, but we currently can't import params there
-    if TICI and HARDWARE.get_device_type() == "tici":
+    if False: #TICI and HARDWARE.get_device_type() == "tici":
      if not os.path.isfile("/persist/comma/living-in-the-moment"):
        if not Path("/data/media").is_mount():
          set_offroad_alert_if_changed("Offroad_StorageMissing", True)
-        else:
-          # check for bad NVMe
-          try:
-            with open("/sys/block/nvme0n1/device/model") as f:
-              model = f.read().strip()
-            if not model.startswith("Samsung SSD 980") and params.get("Offroad_BadNvme") is None:
-              set_offroad_alert_if_changed("Offroad_BadNvme", True)
-              cloudlog.event("Unsupported NVMe", model=model, error=True)
-          except Exception:
-            pass

    # Handle offroad/onroad transition
    should_start = all(onroad_conditions.values())
--- a/system/manager/manager.py
+++ b/system/manager/manager.py
@@ -36,7 +36,7 @@ def get_default_params():

    ("SearchInput", "0"),
    ("GMapKey", "0"),
-    ("MapboxStyle", "0"),    
+    ("MapboxStyle", "0"),


    ("LongitudinalPersonalityMax", "3"),
@@ -87,6 +87,7 @@ def get_default_params():
    ("AutoNaviCountDownMode", "2"),
    ("TurnSpeedControlMode", "1"),
    ("MapTurnSpeedFactor", "90"),
+    ("ModelTurnSpeedFactor", "0"),
    ("StoppingAccel", "0"),
    ("StopDistanceCarrot", "550"),
    ("JLeadFactor3", "0"),
@@ -134,6 +135,8 @@ def get_default_params():
    ("TFollowGap3", "140"),
    ("TFollowGap4", "160"),
    ("DynamicTFollow", "0"),
+    ("AChangeCostStarting", "10"),
+    ("TrafficStopDistanceAdjust", "400"),
    ("DynamicTFollowLC", "100"),
    ("HapticFeedbackWhenSpeedCamera", "0"),
    ("UseLaneLineSpeed", "0"),
@@ -164,6 +167,7 @@ def get_default_params():
    ("CustomSteerDeltaDownLC", "0"),
    ("SpeedFromPCM", "2"),
    ("SteerActuatorDelay", "0"),
+    ("LatSmoothSec", "13"),
    ("MaxTimeOffroadMin", "60"),
    ("DisableDM", "0"),
    ("EnableConnect", "0"),
@@ -298,6 +302,10 @@ def manager_thread() -> None:
    ignore.append("pandad")
  ignore += [x for x in os.getenv("BLOCK", "").split(",") if len(x) > 0]

+  if params.get("HardwareC3xLite"):
+    ignore += ["micd", "soundd", "loggerd"]
+    params.put("RecordAudio", "0")
+
  sm = messaging.SubMaster(['deviceState', 'carParams'], poll='deviceState')
  pm = messaging.PubMaster(['managerState'])

--- a/system/manager/process_config.py
+++ b/system/manager/process_config.py
@@ -75,6 +75,9 @@ def enable_dm(started, params, CP: car.CarParams) -> bool:
 def enable_connect(started, params, CP: car.CarParams) -> bool:
  return params.get_int("EnableConnect") > 0

+def c3x_lite(started: bool, params: Params, CP: car.CarParams) -> bool:
+  return started and params.get_bool("HardwareC3xLite")
+
 procs = [
  DaemonProcess("manage_athenad", "system.athena.manage_athenad", "AthenadPid"),

@@ -100,8 +103,8 @@ procs = [
  NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC),
  NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)),
  PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad),
-  NativeProcess("locationd", "selfdrive/locationd", ["./locationd"], only_onroad),
-  #PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad),
+  NativeProcess("locationd2", "selfdrive/locationd", ["./locationd"], only_onroad),
+  PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad),
  NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False),
  PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad),
  PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad),
@@ -136,6 +139,9 @@ procs = [
  #PythonProcess("fleet_manager", "selfdrive.frogpilot.fleetmanager.fleet_manager", check_fleet, enabled=not PC),
  PythonProcess("fleet_manager", "selfdrive.frogpilot.fleetmanager.fleet_manager", check_fleet),
  PythonProcess("carrot_man", "selfdrive.carrot.carrot_man", always_run),#, enabled=not PC),
+
+  # c3x lite
+  PythonProcess("beep", "selfdrive.controls.beep", c3x_lite, enabled=TICI),
 ]

 managed_processes = {p.name: p for p in procs}