Desires

opendbc_repo/opendbc/car/ford/carcontroller.py

@@ -165,7 +165,7 @@ class CarController(CarControllerBase):
           can_sends.append(starpilot_fordcan.create_lat_ctl2_msg(
             self.packer, self.CAN, 1 if lateral.active else 0,
             lateral.ramp_type, lateral.precision_type,
-            -lateral.curvature, -lateral.curvature_rate, counter))
+            -lateral.curvature, -lateral.curvature_rate, counter, -lateral.path_angle))
         else:
           can_sends.append(starpilot_fordcan.create_lat_ctl_msg(
             self.packer, self.CAN, lateral.active,

opendbc_repo/opendbc/car/ford/interface.py

@@ -8,7 +8,7 @@ from opendbc.car.ford.carcontroller import CarController
 from opendbc.car.ford.carstate import CarState
 from opendbc.car.ford.fordcan import CanBus
 from opendbc.car.ford.radar_interface import RadarInterface
-from opendbc.car.ford.values import CarControllerParams, DBC, Ecu, FordFlags, RADAR, FordSafetyFlags
+from opendbc.car.ford.values import CAR, CarControllerParams, DBC, Ecu, FordFlags, RADAR, FordSafetyFlags
 from opendbc.car.interfaces import CarInterfaceBase
 
 TransmissionType = structs.CarParams.TransmissionType
@@ -63,6 +63,8 @@ class CarInterface(CarInterfaceBase):
 
     if ret.flags & FordFlags.CANFD:
       ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.CANFD.value
+      if candidate == CAR.FORD_MUSTANG_MACH_E_MK1:
+        ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.MACH_E_CURVATURE.value
 
       # TRON (SecOC) platforms are not supported
       # LateralMotionControl2, ACCDATA are 16 bytes on these platforms

opendbc_repo/opendbc/car/ford/tests/test_ford.py

@@ -192,10 +192,12 @@ def test_mach_e_longitudinal_toggle_controls_stock_acc_selection():
   assert not stock.openpilotLongitudinalControl
   assert stock.pcmCruise
   assert not (stock.safetyConfigs[-1].safetyParam & FordSafetyFlags.LONG_CONTROL)
+  assert stock.safetyConfigs[-1].safetyParam & FordSafetyFlags.MACH_E_CURVATURE
 
   assert enhanced.alphaLongitudinalAvailable
   assert enhanced.openpilotLongitudinalControl
   assert enhanced.safetyConfigs[-1].safetyParam & FordSafetyFlags.LONG_CONTROL
+  assert enhanced.safetyConfigs[-1].safetyParam & FordSafetyFlags.MACH_E_CURVATURE
 
 
 def test_mach_e_can_gps_decode():

opendbc_repo/opendbc/car/ford/values.py

@@ -50,6 +50,7 @@ class FordSafetyFlags(IntFlag):
   LONG_CONTROL = 1
   CANFD = 2
   LKA_STEERING = 4
+  MACH_E_CURVATURE = 8
 
 
 class FordFlags(IntFlag):

opendbc_repo/opendbc/safety/modes/ford.h

@@ -96,6 +96,8 @@ static bool ford_lka_steering = false;
 static bool ford_extended_lateral = false;
 static bool ford_longitudinal = false;
 static bool ford_cancel_resume_button = false;
+static bool ford_mach_e_curvature = false;
+static int ford_path_angle_last = 0;
 
 // Curvature rate limits
 #define FORD_LIMITS(limit_lateral_acceleration) {                                               \
@@ -121,10 +123,10 @@ static bool ford_cancel_resume_button = false;
 
 static const AngleSteeringLimits FORD_STEERING_LIMITS = FORD_LIMITS(false);
 
-#define FORD_EXTENDED_LIMITS(limit_lateral_acceleration) {                                      \
+#define FORD_EXTENDED_LIMITS(limit_lateral_acceleration, max_curvature_error) {                 \
   .max_angle = 1000,                                                                            \
   .angle_deg_to_can = 50000,                                                                    \
-  .max_angle_error = 100,                                                                       \
+  .max_angle_error = (max_curvature_error),                                                     \
   .angle_rate_up_lookup = {                                                                     \
     {5., 16., 25.},                                                                             \
     {0.0025, 0.0014, 0.00018}                                                                   \
@@ -140,7 +142,7 @@ static const AngleSteeringLimits FORD_STEERING_LIMITS = FORD_LIMITS(false);
   .inactive_angle_is_zero = true,                                                               \
 }
 
-static const AngleSteeringLimits FORD_EXTENDED_STEERING_LIMITS = FORD_EXTENDED_LIMITS(false);
+static const AngleSteeringLimits FORD_EXTENDED_STEERING_LIMITS = FORD_EXTENDED_LIMITS(false, 100);
 
 static void ford_rx_hook(const CANPacket_t *msg) {
   if (msg->bus == FORD_MAIN_BUS) {
@@ -318,7 +320,8 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
   // Safety check for LateralMotionControl2 action
   if (msg->addr == FORD_LateralMotionControl2) {
     static const AngleSteeringLimits FORD_CANFD_STEERING_LIMITS = FORD_LIMITS(true);
-    static const AngleSteeringLimits FORD_CANFD_EXTENDED_STEERING_LIMITS = FORD_EXTENDED_LIMITS(true);
+    static const AngleSteeringLimits FORD_CANFD_EXTENDED_STEERING_LIMITS = FORD_EXTENDED_LIMITS(true, 100);
+    static const AngleSteeringLimits FORD_MACH_E_CURVATURE_LIMITS = FORD_EXTENDED_LIMITS(true, 300);
 
     // Signal: LatCtl_D2_Rq
     bool steer_control_enabled = ((msg->data[0] >> 4) & 0x7U) != 0U;
@@ -336,9 +339,19 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
     if (ford_extended_lateral) {
       violation |= desired_path_offset != 0;
       violation |= (desired_curvature_rate < -1024) || (desired_curvature_rate > 1023);
-      violation |= desired_path_angle != 0;
+      if (desired_path_angle != 0) {
+        const float speed = vehicle_speed.max / VEHICLE_SPEED_FACTOR;
+        const float curvature = (float)SAFETY_ABS(desired_curvature) / 50000.0f;
+        const float path_angle = (float)SAFETY_ABS(desired_path_angle) / 2000.0f;
+        const float combined_acceleration = (curvature + path_angle / SAFETY_MAX(speed, 1.0f)) * speed * speed;
+        violation |= !ford_mach_e_curvature || !steer_control_enabled || !controls_allowed;
+        violation |= (speed < 3.0f) || (speed >= 8.8f);
+        violation |= (SAFETY_ABS(desired_curvature) < 975) || (SAFETY_ABS(desired_path_angle) > 320);
+        violation |= (desired_curvature * desired_path_angle <= 0) || (combined_acceleration > 2.5f);
+        violation |= SAFETY_ABS(desired_path_angle - ford_path_angle_last) > 110;
+      }
       violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled,
-                                          FORD_CANFD_EXTENDED_STEERING_LIMITS);
+                                          ford_mach_e_curvature ? FORD_MACH_E_CURVATURE_LIMITS : FORD_CANFD_EXTENDED_STEERING_LIMITS);
       if (!steer_control_enabled) {
         violation |= (desired_curvature != 0) || (desired_curvature_rate != 0);
       }
@@ -352,6 +365,8 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
 
     if (violation) {
       tx = false;
+    } else {
+      ford_path_angle_last = desired_path_angle;
     }
   }
 
@@ -406,8 +421,11 @@ static safety_config ford_init(uint16_t param) {
 
   const uint16_t FORD_PARAM_CANFD = 2;
   const uint16_t FORD_PARAM_LKA_STEERING = 4;
+  const uint16_t FORD_PARAM_MACH_E_CURVATURE = 8;
   const bool ford_canfd = GET_FLAG(param, FORD_PARAM_CANFD);
   ford_lka_steering = GET_FLAG(param, FORD_PARAM_LKA_STEERING);
+  ford_mach_e_curvature = ford_canfd && GET_FLAG(param, FORD_PARAM_MACH_E_CURVATURE);
+  ford_path_angle_last = 0;
   ford_extended_lateral = false;
   ford_cancel_resume_button = false;
 

opendbc_repo/opendbc/safety/tests/test_ford.py

@@ -467,6 +467,53 @@ class TestFordCANFDStockSafety(TestFordSafetyBase):
     self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.CANFD)
     self.safety.init_tests()
 
+
+class TestFordMachEExtendedCurvatureSafety(TestFordCANFDStockSafety):
+  def setUp(self):
+    self.packer = CANPackerSafety("ford_lincoln_base_pt")
+    self.safety = libsafety_py.libsafety
+    self.safety.set_safety_hooks(CarParams.SafetyModel.ford,
+                                 FordSafetyFlags.CANFD | FordSafetyFlags.MACH_E_CURVATURE)
+    self.safety.init_tests()
+
+  def test_mach_e_extended_curvature_error(self):
+    self.safety.set_controls_allowed(True)
+    self._reset_curvature_measurement(0.0, 12.0)
+    self.assertTrue(self._tx(self._extended_lka_msg()))
+
+    for curvature, allowed in ((0.0058, True), (0.0062, False), (-0.0058, True), (-0.0062, False)):
+      self._set_prev_desired_angle(curvature)
+      self.assertEqual(allowed, self._tx(self._lat_ctl_msg(True, 0.0, 0.0, curvature, 0.0)))
+
+    self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.02, 0.005, 0.0)))
+
+  def test_mach_e_bounded_path_angle_assist(self):
+    self.safety.set_controls_allowed(True)
+    self._reset_curvature_measurement(0.02, 7.5)
+    self._set_prev_desired_angle(0.02)
+    self.assertTrue(self._tx(self._extended_lka_msg()))
+    for path_angle in (0.055, 0.11, 0.15):
+      self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, path_angle, 0.02, 0.0)))
+    self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.161, 0.02, 0.0)))
+    self.assertTrue(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, 0.02, 0.0)))
+    self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, -0.055, 0.02, 0.0)))
+    self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.055, 0.018, 0.0)))
+    self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.12, 0.02, 0.0)))
+    self._reset_curvature_measurement(0.02, 9.0)
+    self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.055, 0.02, 0.0)))
+
+  def test_other_canfd_fords_keep_original_error(self):
+    self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.CANFD)
+    self.safety.init_tests()
+    self.safety.set_controls_allowed(True)
+    self._reset_curvature_measurement(0.0, 12.0)
+    self.assertTrue(self._tx(self._extended_lka_msg()))
+    self._set_prev_desired_angle(0.0058)
+    self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.0, 0.0058, 0.0)))
+    self._reset_curvature_measurement(0.02, 7.5)
+    self._set_prev_desired_angle(0.02)
+    self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.055, 0.02, 0.0)))
+
 class TestFordStockSafety(TestFordSafetyBase):
   STEER_MESSAGE = MSG_LateralMotionControl
   STOCK_LONGITUDINAL = True

selfdrive/controls/lib/desire_helper.py

@@ -1,4 +1,5 @@
 import json
+from time import monotonic
 
 import numpy as np
 
@@ -12,8 +13,11 @@ LaneChangeDirection = log.LaneChangeDirection
 
 LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
 LANE_CHANGE_TIME_MAX = 10.
-NAV_TURN_DISTANCE_SPEED_BREAKPOINTS = [0.0, 5.0, 10.0]
-NAV_TURN_DISTANCE_BREAKPOINTS = [20.0, 35.0, 55.0]
+NAV_TURN_MAX_SPEED = 14.0
+NAV_TURN_PREVIEW_SECONDS = 6.0
+NAV_TURN_MIN_DISTANCE = 35.0
+NAV_TURN_MAX_DISTANCE = 90.0
+NAV_INSTRUCTION_MAX_AGE = 2.5
 # A driver normally signals an intersection before slowing below the lane-change
 # speed threshold. Use the route to classify that early signal so it does not
 # start a lane change while approaching the matching turn.
@@ -122,7 +126,20 @@ class DesireHelper:
     except (TypeError, ValueError):
       return False
 
-    return 0.0 <= distance <= float(np.interp(carstate.vEgo, NAV_TURN_DISTANCE_SPEED_BREAKPOINTS, NAV_TURN_DISTANCE_BREAKPOINTS))
+    preview_distance = float(np.clip(
+      max(float(carstate.vEgo), 0.0) * NAV_TURN_PREVIEW_SECONDS,
+      NAV_TURN_MIN_DISTANCE,
+      NAV_TURN_MAX_DISTANCE,
+    ))
+    return 0.0 <= distance <= preview_distance
+
+  def _nav_instruction_is_fresh(self):
+    try:
+      updated_at = float(self._nav_instruction_state["updatedAtMonotonic"])
+    except (KeyError, TypeError, ValueError):
+      return False
+    age = monotonic() - updated_at
+    return 0.0 <= age <= NAV_INSTRUCTION_MAX_AGE
 
   @staticmethod
   def _nav_turn_signal_matches(carstate, nav_instruction_state):
@@ -234,7 +251,7 @@ class DesireHelper:
     self.nav_lane_positioning_allowed = bool(
       getattr(starpilot_toggles, "nav_lane_positioning_allowed", self.nav_lane_positioning_allowed)
     )
-    if not self.nav_desires_allowed or not lateral_active or not bool(self._nav_instruction_state.get("valid", False)):
+    if not self.nav_desires_allowed or not lateral_active or not bool(self._nav_instruction_state.get("valid", False)) or not self._nav_instruction_is_fresh():
       return log.Desire.none
 
     maneuver_distance = self._nav_instruction_state.get("maneuverDistance", 0.0)
@@ -265,14 +282,14 @@ class DesireHelper:
       if self.turn_stop_hold:
         return log.Desire.none
       turn_allowed = carstate.leftBlinker and not carstate.rightBlinker and not carstate.leftBlindspot
-      turn_allowed &= carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill
+      turn_allowed &= 0.0 <= carstate.vEgo < NAV_TURN_MAX_SPEED and not carstate.standstill
       if turn_allowed and self._nav_turn_is_imminent(carstate, maneuver_distance):
         return log.Desire.turnLeft
     elif modifier in ("right", "sharpRight"):
       if self.turn_stop_hold:
         return log.Desire.none
       turn_allowed = carstate.rightBlinker and not carstate.leftBlinker and not carstate.rightBlindspot
-      turn_allowed &= carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill
+      turn_allowed &= 0.0 <= carstate.vEgo < NAV_TURN_MAX_SPEED and not carstate.standstill
       if turn_allowed and self._nav_turn_is_imminent(carstate, maneuver_distance):
         return log.Desire.turnRight
 
@@ -289,7 +306,7 @@ class DesireHelper:
 
     self._update_nav_params()
     self.nav_desires_allowed = bool(getattr(starpilot_toggles, "nav_desires_allowed", self.nav_desires_allowed))
-    nav_turn_signal = self.nav_desires_allowed and self._nav_turn_signal_matches(carstate, self._nav_instruction_state)
+    nav_turn_signal = self.nav_desires_allowed and self._nav_instruction_is_fresh() and self._nav_turn_signal_matches(carstate, self._nav_instruction_state)
 
     stop_imminent = (bool(getattr(starpilotPlan, "redLight", False))
                      or bool(getattr(starpilotPlan, "forcingStop", False))
@@ -409,3 +426,5 @@ class DesireHelper:
     nav_desire = self._navigation_desire(carstate, lateral_active, starpilotPlan, starpilot_toggles)
     if nav_desire != log.Desire.none and self.lane_change_state == LaneChangeState.off:
       self.desire = nav_desire
+      if nav_desire in (log.Desire.turnLeft, log.Desire.turnRight):
+        self.turn_direction = nav_desire

selfdrive/controls/lib/latcontrol_torque.py

@@ -647,6 +647,9 @@ class LatControlTorque(LatControl):
         output_torque *= tucson_4th_gen_center_taper
       elif genesis_g70_active:
         output_torque *= genesis_g70_center_output_taper
+        output_torque *= get_genesis_g70_highway_turn_in_output_scale(
+          output_torque, setpoint, measurement, desired_lateral_jerk, CS.vEgo,
+        )
         output_torque *= get_genesis_g70_high_speed_error_scale(
           setpoint, measurement, desired_lateral_jerk, CS.vEgo,
         )

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -276,7 +276,7 @@ GENESIS_GV70_OUTPUT_SMOOTHING_UNWIND_PHASE_WIDTH = 0.08
 GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_LAT = 0.55
 GENESIS_GV70_OUTPUT_SMOOTHING_DIRECTION_CHANGE_RC = 0.065
 GENESIS_GV70_HIGHWAY_STABILIZER_SPEED_BP = [40.0 * CV.MPH_TO_MS, 50.0 * CV.MPH_TO_MS]
-GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP = [0.45, 0.65]
+GENESIS_GV70_HIGHWAY_STABILIZER_CENTER_LAT_BP = [0.75, 1.0]
 GENESIS_GV70_HIGHWAY_STABILIZER_BASELINE_RC = 0.85
 GENESIS_GV70_HIGHWAY_STABILIZER_BLEND_RC = 0.35
 GENESIS_GV70_HIGHWAY_STABILIZER_REVERSAL_LAT = 0.06
@@ -288,6 +288,11 @@ GENESIS_G70_FRICTION_THRESHOLD_GAIN = 0.10
 GENESIS_G70_CURVE_TURN_IN_JERK_REDUCTION = 0.50
 GENESIS_G70_CURVE_TURN_IN_SPEED_BP = [20.0, 25.0]
 GENESIS_G70_CURVE_TURN_IN_LAT_BP = [0.35, 0.70]
+GENESIS_G70_HIGHWAY_TURN_IN_OUTPUT_REDUCTION = 0.12
+GENESIS_G70_HIGHWAY_TURN_IN_SPEED_BP = [26.0, 32.0]
+GENESIS_G70_HIGHWAY_TURN_IN_LAT_BP = [0.70, 1.10]
+GENESIS_G70_HIGHWAY_TURN_IN_JERK_BP = [0.25, 0.60]
+GENESIS_G70_HIGHWAY_TURN_IN_TRACKING_BP = [0.70, 0.90, 1.10]
 GENESIS_G70_FRICTION_THRESHOLD_SPEED_BP = [10.0, 20.0]
 GENESIS_G70_FRICTION_THRESHOLD_SPEED_V = [1.0, 2.0]
 GENESIS_G70_FRICTION_SPEED_ONSET = 10.0
@@ -3506,6 +3511,20 @@ def get_genesis_g70_high_speed_error_scale(setpoint: float, measured_lateral_acc
   return 1.0 - reduction
 
 
+def get_genesis_g70_highway_turn_in_output_scale(output_torque: float, setpoint: float,
+                                                   measured_lateral_accel: float,
+                                                   desired_lateral_jerk: float, v_ego: float) -> float:
+  if (setpoint * desired_lateral_jerk <= 0.0 or setpoint * measured_lateral_accel <= 0.0 or
+      output_torque * setpoint >= 0.0):
+    return 1.0
+  speed_weight = np.interp(v_ego, GENESIS_G70_HIGHWAY_TURN_IN_SPEED_BP, [0.0, 1.0])
+  curve_weight = np.interp(abs(setpoint), GENESIS_G70_HIGHWAY_TURN_IN_LAT_BP, [0.0, 1.0])
+  jerk_weight = np.interp(abs(desired_lateral_jerk), GENESIS_G70_HIGHWAY_TURN_IN_JERK_BP, [0.0, 1.0])
+  tracking_weight = np.interp(abs(measured_lateral_accel / setpoint),
+                              GENESIS_G70_HIGHWAY_TURN_IN_TRACKING_BP, [0.0, 1.0, 0.0])
+  return 1.0 - GENESIS_G70_HIGHWAY_TURN_IN_OUTPUT_REDUCTION * speed_weight * curve_weight * jerk_weight * tracking_weight
+
+
 def get_genesis_g70_stabilized_output(output_torque: float, prev_output_torque: float,
                                       desired_lateral_accel: float, measured_lateral_accel: float,
                                       desired_lateral_jerk: float, v_ego: float, dt: float) -> float:

selfdrive/controls/tests/test_g70_transition_continuity.py

@@ -54,3 +54,23 @@ def test_full_overshoot_blend_preserves_large_error_protection():
   assert tunes.get_genesis_g70_overshoot_blend(0.8, 1.0) == 1.0
   assert tunes.get_genesis_g70_overshoot_blend(-0.8, -1.0) == 1.0
   assert tunes.get_genesis_g70_unwind_ff_scale(0.8, 1.0, -0.5, 30) < 1.0
+
+
+@pytest.mark.parametrize('direction', [-1, 1])
+def test_highway_turn_in_taper_only_near_tracking_target(direction):
+  scale = tunes.get_genesis_g70_highway_turn_in_output_scale
+  args = (-direction * 0.35, direction * 1.2, direction * 1.08, direction * 0.6, 32.0)
+  assert scale(*args) == pytest.approx(0.88)
+  assert scale(*args[:-1], 20.0) == 1.0
+  assert scale(args[0], args[1], direction * 0.5, args[3], args[4]) == 1.0
+  assert scale(args[0], args[1], direction * 1.32, args[3], args[4]) == 1.0
+  assert scale(args[0], args[1], args[2], -args[3], args[4]) == 1.0
+  assert scale(-args[0], args[1], args[2], args[3], args[4]) == 1.0
+
+
+@pytest.mark.parametrize('direction', [-1, 1])
+def test_highway_turn_in_taper_continuous_at_tracking_boundary(direction):
+  scale = tunes.get_genesis_g70_highway_turn_in_output_scale
+  values = [scale(-direction * 0.35, direction * 1.2, direction * (1.2 + epsilon),
+                  direction * 0.6, 32.0) for epsilon in [-1e-7, 1e-7]]
+  assert abs(values[1] - values[0]) < 1e-5

selfdrive/controls/tests/test_gv70_highway_stabilizer.py

@@ -41,9 +41,21 @@ def test_repeated_highway_reversals_are_bounded_and_damped():
   assert np.max(np.abs(np.array(raw) - np.array(shaped))) <= 0.20 + 1e-6
 
 
+def test_repeated_moderate_curve_reversals_are_damped():
+  stabilizer = GenesisGV70HighwayCommandStabilizer()
+  raw, shaped = [], []
+  for i in range(1200):
+    accel = 0.55 + 0.25 * math.sin(2.0 * math.pi * 0.5 * i * 0.01)
+    raw.append(accel)
+    shaped.append(update_accel(stabilizer, accel))
+
+  assert np.std(shaped[600:]) < 0.75 * np.std(raw[600:])
+  assert np.max(np.abs(np.array(raw) - np.array(shaped))) <= 0.20 + 1e-6
+
+
 def test_sustained_curve_is_unchanged():
   stabilizer = GenesisGV70HighwayCommandStabilizer()
-  curve = np.concatenate((np.linspace(0.0, 0.55, 150), np.full(300, 0.55), np.linspace(0.55, 0.0, 150)))
+  curve = np.concatenate((np.linspace(0.0, 0.8, 150), np.full(300, 0.8), np.linspace(0.8, 0.0, 150)))
   for accel in curve:
     assert update_accel(stabilizer, float(accel)) == pytest.approx(accel)
 
@@ -55,7 +67,7 @@ def test_strong_turn_and_driver_input_reset_stabilizer():
     update_accel(stabilizer, accel)
 
   for _ in range(100):
-    assert update_accel(stabilizer, 0.8) == pytest.approx(0.8)
+    assert update_accel(stabilizer, 1.2) == pytest.approx(1.2)
   for _ in range(100):
     assert update_accel(stabilizer, 0.4) == pytest.approx(0.4)
 

selfdrive/controls/tests/test_navigation_desires.py

@@ -1,4 +1,5 @@
 from types import SimpleNamespace
+from time import monotonic
 
 from cereal import log
 
@@ -51,7 +52,7 @@ def test_nav_desires_keep_left_when_route_requests_it():
   helper = DesireHelper()
   helper.nav_desires_allowed = True
   helper._update_nav_params = lambda: None
-  helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightLeft"}
+  helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightLeft", "updatedAtMonotonic": monotonic()}
 
   helper.update(
     make_car_state(vEgo=20.0, steeringPressed=True, steeringTorque=1.0),
@@ -68,7 +69,10 @@ def test_nav_desires_turn_right_below_lane_change_speed():
   helper = DesireHelper()
   helper.nav_desires_allowed = True
   helper._update_nav_params = lambda: None
-  helper._nav_instruction_state = {"valid": True, "maneuverType": "turn", "maneuverModifier": "right", "maneuverDistance": 10.0}
+  helper._nav_instruction_state = {
+    "valid": True, "maneuverType": "turn", "maneuverModifier": "right",
+    "maneuverDistance": 10.0, "updatedAtMonotonic": monotonic(),
+  }
 
   helper.update(
     make_car_state(vEgo=5.0, rightBlinker=True),
@@ -84,7 +88,10 @@ def test_nav_desires_turn_right_below_lane_change_speed():
 def test_nav_desires_turn_preview_starts_before_last_second():
   helper = DesireHelper()
   helper._update_nav_params = lambda: None
-  helper._nav_instruction_state = {"valid": True, "maneuverType": "turn", "maneuverModifier": "right", "maneuverDistance": 50.0}
+  helper._nav_instruction_state = {
+    "valid": True, "maneuverType": "turn", "maneuverModifier": "right",
+    "maneuverDistance": 50.0, "updatedAtMonotonic": monotonic(),
+  }
 
   helper.update(
     make_car_state(vEgo=10.5, rightBlinker=True),
@@ -98,17 +105,73 @@ def test_nav_desires_turn_preview_starts_before_last_second():
   assert helper.lane_change_state == LaneChangeState.off
 
 
+def test_routed_turn_replays_signaled_approach_independently_of_lane_change_setting():
+  samples = (
+    (82.6, 13.64, True, log.Desire.none),
+    (69.1, 13.15, True, log.Desire.turnRight),
+    (56.3, 13.18, True, log.Desire.turnRight),
+    (43.1, 13.00, False, log.Desire.none),
+  )
+  for lane_change_speed in (2.777777, 11.1):
+    helper = DesireHelper()
+    helper._update_nav_params = lambda: None
+    toggles = make_toggles(minimum_lane_change_speed=lane_change_speed)
+    for distance, speed, right_blinker, expected in samples:
+      helper._nav_instruction_state = {
+        "valid": True,
+        "maneuverType": "turn",
+        "maneuverModifier": "right",
+        "maneuverDistance": distance,
+        "updatedAtMonotonic": monotonic(),
+      }
+      helper.update(
+        make_car_state(vEgo=speed, rightBlinker=right_blinker),
+        True,
+        0.0,
+        make_plan(),
+        toggles,
+      )
+      assert helper.desire == expected
+      assert helper.lane_change_state == LaneChangeState.off
+      assert helper.turn_direction == expected
+
+
+def test_stale_route_instruction_cannot_request_turn_or_suppress_lane_change():
+  helper = DesireHelper()
+  helper._update_nav_params = lambda: None
+  helper._nav_instruction_state = {
+    "valid": True,
+    "maneuverType": "turn",
+    "maneuverModifier": "right",
+    "maneuverDistance": 55.0,
+    "updatedAtMonotonic": monotonic() - 10.0,
+  }
+  helper.update(
+    make_car_state(vEgo=13.0, rightBlinker=True),
+    True,
+    0.0,
+    make_plan(),
+    make_toggles(minimum_lane_change_speed=2.777777),
+  )
+
+  assert helper.desire == log.Desire.none
+  assert helper.lane_change_state == LaneChangeState.preLaneChange
+
+
 def test_nav_desires_turn_preview_is_bounded_and_requires_matching_signal():
   for distance, blinker, speed, maneuver_type in (
     (65.0, True, 10.5, "turn"),
     (-1.0, True, 10.5, "turn"),
     (50.0, False, 10.5, "turn"),
-    (50.0, True, 11.2, "turn"),
+    (50.0, True, 14.2, "turn"),
     (50.0, True, 10.5, "arrive"),
   ):
     helper = DesireHelper()
     helper._update_nav_params = lambda: None
-    helper._nav_instruction_state = {"valid": True, "maneuverType": maneuver_type, "maneuverModifier": "right", "maneuverDistance": distance}
+    helper._nav_instruction_state = {
+      "valid": True, "maneuverType": maneuver_type, "maneuverModifier": "right",
+      "maneuverDistance": distance, "updatedAtMonotonic": monotonic(),
+    }
 
     helper.update(
       make_car_state(vEgo=speed, rightBlinker=blinker),
@@ -124,7 +187,10 @@ def test_nav_desires_turn_preview_is_bounded_and_requires_matching_signal():
 def test_nav_desires_turn_preview_respects_stop_hold():
   helper = DesireHelper()
   helper._update_nav_params = lambda: None
-  helper._nav_instruction_state = {"valid": True, "maneuverType": "turn", "maneuverModifier": "right", "maneuverDistance": 20.0}
+  helper._nav_instruction_state = {
+    "valid": True, "maneuverType": "turn", "maneuverModifier": "right",
+    "maneuverDistance": 20.0, "updatedAtMonotonic": monotonic(),
+  }
 
   helper.update(
     make_car_state(vEgo=5.0, rightBlinker=True),
@@ -143,7 +209,10 @@ def test_nav_desires_turn_requires_matching_blinker():
     helper = DesireHelper()
     helper.nav_desires_allowed = True
     helper._update_nav_params = lambda: None
-    helper._nav_instruction_state = {"valid": True, "maneuverType": "turn", "maneuverModifier": modifier, "maneuverDistance": 10.0}
+    helper._nav_instruction_state = {
+      "valid": True, "maneuverType": "turn", "maneuverModifier": modifier,
+      "maneuverDistance": 10.0, "updatedAtMonotonic": monotonic(),
+    }
 
     helper.update(
       make_car_state(vEgo=5.0, **{opposite_blinker: True}),
@@ -160,7 +229,10 @@ def test_nav_desires_turn_right_waits_until_turn_is_close():
   helper = DesireHelper()
   helper.nav_desires_allowed = True
   helper._update_nav_params = lambda: None
-  helper._nav_instruction_state = {"valid": True, "maneuverType": "turn", "maneuverModifier": "right", "maneuverDistance": 300.0}
+  helper._nav_instruction_state = {
+    "valid": True, "maneuverType": "turn", "maneuverModifier": "right",
+    "maneuverDistance": 300.0, "updatedAtMonotonic": monotonic(),
+  }
 
   helper.update(
     make_car_state(vEgo=5.0),
@@ -178,6 +250,7 @@ def test_matching_routed_turn_does_not_start_lane_change_above_threshold():
   helper._update_nav_params = lambda: None
   helper._nav_instruction_state = {
     "valid": True,
+    "updatedAtMonotonic": monotonic(),
     "maneuverType": "turn",
     "maneuverModifier": "right",
     "maneuverDistance": 111.0,
@@ -201,6 +274,7 @@ def test_distant_routed_turn_does_not_block_lane_change():
   helper._update_nav_params = lambda: None
   helper._nav_instruction_state = {
     "valid": True,
+    "updatedAtMonotonic": monotonic(),
     "maneuverType": "turn",
     "maneuverModifier": "right",
     "maneuverDistance": 794.0,
@@ -223,6 +297,7 @@ def test_matching_routed_turn_cancels_pending_lane_change_before_it_starts():
   helper._update_nav_params = lambda: None
   helper._nav_instruction_state = {
     "valid": True,
+    "updatedAtMonotonic": monotonic(),
     "maneuverType": "turn",
     "maneuverModifier": "left",
     "maneuverDistance": 125.0,
@@ -249,6 +324,7 @@ def test_nav_desires_off_ramp_lane_guidance_becomes_keep_right():
   helper._update_nav_params = lambda: None
   helper._nav_instruction_state = {
     "valid": True,
+    "updatedAtMonotonic": monotonic(),
     "maneuverType": "off ramp",
     "maneuverModifier": "right",
     "activeLaneDirection": "slightRight",
@@ -272,6 +348,7 @@ def test_nav_desires_off_ramp_lane_guidance_waits_until_split_is_close():
   helper._update_nav_params = lambda: None
   helper._nav_instruction_state = {
     "valid": True,
+    "updatedAtMonotonic": monotonic(),
     "maneuverType": "off ramp",
     "maneuverModifier": "right",
     "activeLaneDirection": "slightRight",
@@ -295,6 +372,7 @@ def test_nav_desires_ambiguous_off_ramp_waits_longer_before_keep_right():
   helper._update_nav_params = lambda: None
   helper._nav_instruction_state = {
     "valid": True,
+    "updatedAtMonotonic": monotonic(),
     "maneuverType": "off ramp",
     "maneuverModifier": "right",
     "activeLaneDirection": "slightRight",
@@ -319,6 +397,7 @@ def test_nav_desires_edge_exit_lane_with_shared_transition_lane_does_not_keep_ri
   helper._update_nav_params = lambda: None
   helper._nav_instruction_state = {
     "valid": True,
+    "updatedAtMonotonic": monotonic(),
     "maneuverType": "off ramp",
     "maneuverModifier": "right",
     "activeLaneDirection": "slightRight",
@@ -346,6 +425,7 @@ def test_nav_desires_wide_highway_edge_exit_lane_keeps_right():
   helper._update_nav_params = lambda: None
   helper._nav_instruction_state = {
     "valid": True,
+    "updatedAtMonotonic": monotonic(),
     "maneuverType": "off ramp",
     "maneuverModifier": "right",
     "activeLaneDirection": "slightRight",
@@ -373,6 +453,7 @@ def test_nav_desires_shared_transition_lane_keeps_when_active_lane_is_not_outerm
   helper._update_nav_params = lambda: None
   helper._nav_instruction_state = {
     "valid": True,
+    "updatedAtMonotonic": monotonic(),
     "maneuverType": "off ramp",
     "maneuverModifier": "right",
     "activeLaneDirection": "slightRight",
@@ -399,6 +480,7 @@ def test_nav_desires_ambiguous_fork_slight_right_only_keeps_close_to_split():
   helper._update_nav_params = lambda: None
   helper._nav_instruction_state = {
     "valid": True,
+    "updatedAtMonotonic": monotonic(),
     "maneuverType": "fork",
     "maneuverModifier": "slightRight",
     "activeLaneDirection": "slightRight",
@@ -423,6 +505,7 @@ def test_nav_desires_ambiguous_fork_slight_right_does_not_nudge_too_early():
   helper._update_nav_params = lambda: None
   helper._nav_instruction_state = {
     "valid": True,
+    "updatedAtMonotonic": monotonic(),
     "maneuverType": "fork",
     "maneuverModifier": "slightRight",
     "activeLaneDirection": "slightRight",
@@ -447,6 +530,7 @@ def test_nav_desires_fork_with_active_straight_lane_does_not_turn_left():
   helper._update_nav_params = lambda: None
   helper._nav_instruction_state = {
     "valid": True,
+    "updatedAtMonotonic": monotonic(),
     "maneuverType": "fork",
     "maneuverModifier": "left",
     "activeLaneDirection": "straight",
@@ -468,7 +552,7 @@ def test_nav_desires_do_not_override_lane_change_state_machine():
   helper = DesireHelper()
   helper.nav_desires_allowed = True
   helper._update_nav_params = lambda: None
-  helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightRight"}
+  helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightRight", "updatedAtMonotonic": monotonic()}
   helper.lane_change_state = LaneChangeState.laneChangeStarting
   helper.lane_change_direction = LaneChangeDirection.left
   helper.lane_change_ll_prob = 0.5
@@ -581,7 +665,7 @@ def test_nav_desires_nudgeless_only_when_engaged_blocks_keep_when_aol_only():
   helper = DesireHelper()
   helper.nav_desires_allowed = True
   helper._update_nav_params = lambda: None
-  helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightLeft"}
+  helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightLeft", "updatedAtMonotonic": monotonic()}
 
   helper.update(
     make_car_state(vEgo=20.0),
@@ -651,7 +735,7 @@ def test_turn_desire_released_after_stop_completes():
 def test_nav_desires_disabled_leave_desire_unchanged():
   helper = DesireHelper()
   helper._update_nav_params = lambda: None
-  helper._nav_instruction_state = {"valid": True, "maneuverModifier": "left"}
+  helper._nav_instruction_state = {"valid": True, "maneuverModifier": "left", "updatedAtMonotonic": monotonic()}
 
   helper.update(
     make_car_state(vEgo=5.0),
@@ -667,7 +751,7 @@ def test_nav_desires_disabled_leave_desire_unchanged():
 def test_disabling_nav_desires_clears_active_route_desire_immediately():
   helper = DesireHelper()
   helper._update_nav_params = lambda: None
-  helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightRight"}
+  helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightRight", "updatedAtMonotonic": monotonic()}
   car_state = make_car_state(vEgo=20.0, steeringPressed=True, steeringTorque=-1.0)
   plan = make_plan(laneWidthRight=4.2)
 
@@ -681,7 +765,7 @@ def test_disabling_nav_desires_clears_active_route_desire_immediately():
 def test_nav_lane_positioning_requires_driver_confirmation():
   helper = DesireHelper()
   helper._update_nav_params = lambda: None
-  helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightRight"}
+  helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightRight", "updatedAtMonotonic": monotonic()}
 
   helper.update(
     make_car_state(vEgo=20.0),

selfdrive/ui/soundd.py

@@ -33,6 +33,18 @@ VOLUME_BASE = 20
 if HARDWARE.get_device_type() in ("tici", "tizi"):
   VOLUME_BASE = 10
 
+VOLUME_SETTINGS_REFRESH_INTERVAL = 0.25
+VOLUME_SETTING_KEYS = (
+  "BelowSteerSpeedVolume",
+  "DisengageVolume",
+  "EngageVolume",
+  "PromptVolume",
+  "PromptDistractedVolume",
+  "RefuseVolume",
+  "WarningImmediateVolume",
+  "WarningSoftVolume",
+)
+
 AudibleAlert = log.SelfdriveState.AudibleAlert
 
 StarPilotAudibleAlert = custom.StarPilotCarControl.HUDControl.AudibleAlert
@@ -68,6 +80,23 @@ def should_mute_turn_steering_limit_alert(alert_type: str, v_ego: float, mute_be
   )
 
 
+def read_volume_settings(params):
+  def read_int(key, minimum=0):
+    try:
+      value = int(params.get_int(key, return_default=True, default=101))
+    except (TypeError, ValueError, OSError):
+      value = 101
+    return max(minimum, min(value, 101))
+
+  return {
+    "alert_volume_controller": params.get_bool("AlertVolumeControl"),
+    **{
+      key: read_int(key, minimum=25 if key in ("WarningImmediateVolume", "WarningSoftVolume") else 0)
+      for key in VOLUME_SETTING_KEYS
+    },
+  }
+
+
 sound_list: dict[int, tuple[str, int | None, float]] = {
   GPU_MODEL_READY_ALERT: ("model_ready.wav", 1, MAX_VOLUME),
   # AudibleAlert, file name, play count (none for infinite)
@@ -127,6 +156,9 @@ class Soundd:
     self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)
 
     self.params_memory = Params(memory=True)
+    self.volume_params = Params(return_defaults=True)
+    self.volume_settings = read_volume_settings(self.volume_params)
+    self.last_volume_settings_refresh = 0.0
     from openpilot.starpilot.common.gpu_model_ready_sound import GpuModelReadyChime
     self.model_ready_chime = GpuModelReadyChime()
     self.model_ready_params = Params(return_defaults=True)
@@ -362,7 +394,7 @@ class Soundd:
 
   def get_volume_override(self):
     if self.current_alert_type.startswith("belowSteerSpeed/"):
-      return self.starpilot_toggles.below_steer_speed_volume / 100.0
+      return self.volume_settings["BelowSteerSpeedVolume"] / 100.0
 
     return self.volume_map.get(self.current_alert, 1.01)
 
@@ -410,6 +442,7 @@ class Soundd:
 
         while True:
           sm.update(0)
+          self.refresh_volume_settings()
           self.update_bluetooth_audio()
 
           if self.pending_stream_status is not None:
@@ -422,7 +455,7 @@ class Soundd:
             self.auto_volume = self.calculate_volume(float(self.spl_filter_weighted.x))
             self.current_volume = self.auto_volume
 
-            if self.starpilot_toggles.alert_volume_controller:
+            if self.volume_settings["alert_volume_controller"]:
               self.current_volume = 0.0
 
           self.get_audible_alert(sm)
@@ -436,7 +469,7 @@ class Soundd:
               float(getattr(self.starpilot_toggles, "turn_steering_limit_mute_speed", 0.0)),
             ):
               self.current_volume = 0.0
-            elif self.starpilot_toggles.alert_volume_controller:
+            elif self.volume_settings["alert_volume_controller"]:
               self.current_volume = self.get_volume_override()
               if self.current_volume == 1.01:
                 self.current_volume = self.auto_volume
@@ -465,25 +498,43 @@ class Soundd:
           except Exception:
             cloudlog.exception("soundd: failed to close stream")
 
-  def update_starpilot_sounds(self, sd=None, stream=None):
+  def refresh_volume_settings(self, now=None):
+    now = time.monotonic() if now is None else now
+    if now - self.last_volume_settings_refresh < VOLUME_SETTINGS_REFRESH_INTERVAL:
+      return False
+
+    self.last_volume_settings_refresh = now
+    volume_settings = read_volume_settings(self.volume_params)
+    if volume_settings == self.volume_settings:
+      return False
+
+    self.volume_settings = volume_settings
+    self.update_volume_map()
+    return True
+
+  def update_volume_map(self):
+    settings = self.volume_settings
     self.volume_map = {
-      AudibleAlert.engage: self.starpilot_toggles.engage_volume / 100.0,
-      AudibleAlert.disengage: self.starpilot_toggles.disengage_volume / 100.0,
-      AudibleAlert.refuse: self.starpilot_toggles.refuse_volume / 100.0,
+      AudibleAlert.engage: settings["EngageVolume"] / 100.0,
+      AudibleAlert.disengage: settings["DisengageVolume"] / 100.0,
+      AudibleAlert.refuse: settings["RefuseVolume"] / 100.0,
 
-      AudibleAlert.prompt: self.starpilot_toggles.prompt_volume / 100.0,
-      AudibleAlert.promptRepeat: self.starpilot_toggles.prompt_volume / 100.0,
-      AudibleAlert.promptDistracted: self.starpilot_toggles.promptDistracted_volume / 100.0,
+      AudibleAlert.prompt: settings["PromptVolume"] / 100.0,
+      AudibleAlert.promptRepeat: settings["PromptVolume"] / 100.0,
+      AudibleAlert.promptDistracted: settings["PromptDistractedVolume"] / 100.0,
 
-      AudibleAlert.preAlert: self.starpilot_toggles.promptDistracted_volume / 100.0,
+      AudibleAlert.preAlert: settings["PromptDistractedVolume"] / 100.0,
 
-      AudibleAlert.warningSoft: self.starpilot_toggles.warningSoft_volume / 100.0,
-      AudibleAlert.warningImmediate: self.starpilot_toggles.warningImmediate_volume / 100.0,
+      AudibleAlert.warningSoft: settings["WarningSoftVolume"] / 100.0,
+      AudibleAlert.warningImmediate: settings["WarningImmediateVolume"] / 100.0,
 
-      starpilot_alert_key(StarPilotAudibleAlert.goat): self.starpilot_toggles.prompt_volume / 100.0,
-      starpilot_alert_key(StarPilotAudibleAlert.startup): self.starpilot_toggles.engage_volume / 100.0
+      starpilot_alert_key(StarPilotAudibleAlert.goat): settings["PromptVolume"] / 100.0,
+      starpilot_alert_key(StarPilotAudibleAlert.startup): settings["EngageVolume"] / 100.0
     }
 
+  def update_starpilot_sounds(self, sd=None, stream=None):
+    self.update_volume_map()
+
     for sound in sound_list:
       if sound not in self.volume_map:
         self.volume_map[sound] = 1.01

selfdrive/ui/tests/test_soundd.py

@@ -7,6 +7,7 @@ from openpilot.selfdrive.ui.soundd import (
   Soundd,
   check_selfdrive_timeout_alert,
   is_turn_steering_limit_alert,
+  read_volume_settings,
   should_mute_turn_steering_limit_alert,
   starpilot_alert_key,
 )
@@ -20,6 +21,39 @@ StarPilotAudibleAlert = custom.StarPilotCarControl.HUDControl.AudibleAlert
 
 
 class TestSoundd:
+  def test_volume_settings_are_read_as_percentages(self):
+    class FakeParams:
+      values = {
+        "AlertVolumeControl": True,
+        "BelowSteerSpeedVolume": 0,
+        "PromptVolume": 20,
+        "WarningSoftVolume": 25,
+        "WarningImmediateVolume": 101,
+      }
+
+      def get_bool(self, key):
+        return self.values.get(key, False)
+
+      def get_int(self, key, return_default=False, default=101):
+        return self.values.get(key, default)
+
+    params = FakeParams()
+    settings = read_volume_settings(params)
+    assert settings["alert_volume_controller"] is True
+    assert settings["BelowSteerSpeedVolume"] == 0
+    assert settings["PromptVolume"] == 20
+    assert settings["WarningImmediateVolume"] == 101
+
+    soundd = Soundd.__new__(Soundd)
+    soundd.volume_params = params
+    soundd.volume_settings = settings
+    soundd.last_volume_settings_refresh = 0.0
+    soundd.update_volume_map()
+
+    params.values["PromptVolume"] = 35
+    assert soundd.refresh_volume_settings(now=1.0)
+    assert soundd.volume_map[AudibleAlert.promptRepeat] == 0.35
+
   def test_does_not_consume_car_state_reader(self):
     assert "carState" not in SOUNDD_SERVICES
     assert "starpilotSelfdriveState" in SOUNDD_SERVICES

starpilot/car/ford/fordcan.py

@@ -32,13 +32,13 @@ def create_lat_ctl_msg(packer, CAN: CanBus, active: bool, ramp_type: int, precis
 
 
 def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, ramp_type: int, precision_type: int,
-                        curvature: float, curvature_rate: float, counter: int):
+                        curvature: float, curvature_rate: float, counter: int, path_angle: float = 0.0):
   values = {
     "LatCtl_D2_Rq": mode,
     "LatCtlRampType_D_Rq": ramp_type,
     "LatCtlPrecision_D_Rq": precision_type,
     "LatCtlPathOffst_L_Actl": 0.0,
-    "LatCtlPath_An_Actl": 0.0,
+    "LatCtlPath_An_Actl": path_angle,
     "LatCtlCurv_No_Actl": curvature,
     "LatCtlCrv_NoRate2_Actl": curvature_rate,
     "HandsOffCnfm_B_Rq": 0,

starpilot/car/ford/lateral.py

@@ -78,6 +78,13 @@ MACH_E_SHARP_DIRECTION_CHANGE_MIN_ACCEL = 1.8
 MACH_E_SHARP_DIRECTION_CHANGE_FULL_ACCEL = 2.2
 MACH_E_SHARP_DIRECTION_CHANGE_MIN_LAG_CURVATURE = -0.0005
 MACH_E_SHARP_DIRECTION_CHANGE_FULL_LAG_CURVATURE = 0.0008
+MACH_E_UNDERSTEER_ERROR_MAX = 0.006
+MACH_E_UNDERSTEER_ERROR_MIN_DEFICIT = 0.002
+MACH_E_UNDERSTEER_ERROR_FULL_DEFICIT = 0.004
+MACH_E_PATH_ANGLE_MAX = 0.16
+MACH_E_PATH_ANGLE_STEP = 0.055
+MACH_E_PATH_ANGLE_FADE_START_SPEED = 8.0
+MACH_E_PATH_ANGLE_MAX_SPEED = 8.8
 FORD_CURVATURE_LOOKAHEAD = {
   CAR.FORD_EXPLORER_MK6: 0.20,
 }
@@ -99,6 +106,7 @@ MANUAL_TURN_RECOVERY_SECONDS = 0.25
 class FordLateralResult:
   curvature: float = 0.0
   curvature_rate: float = 0.0
+  path_angle: float = 0.0
   ramp_type: int = 0
   precision_type: int = 1
   active: bool = False
@@ -162,6 +170,7 @@ class FordLateralController:
     self.manual_turn_direction = 0.0
     self.curvature_samples = deque(maxlen=max(2, round(0.3 / STEER_DT)))
     self.curvature_last = 0.0
+    self.path_angle_last = 0.0
     self.desired_curvature_last = 0.0
     self._frame = 0
     self._update_params()
@@ -211,6 +220,37 @@ class FordLateralController:
   def _current_curvature(CS) -> float:
     return -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
 
+  def _curvature_error_limit(self, requested: float, desired: float, current: float, v_ego: float,
+                             steering_pressed: bool, lane_change: bool) -> float:
+    base = CarControllerParams.CURVATURE_ERROR
+    if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or
+        steering_pressed or lane_change or requested * desired <= 0.0 or abs(desired) < 0.003):
+      return base
+    deficit = np.sign(desired) * (requested - current)
+    speed_weight = float(np.interp(v_ego, [9.0, 10.0, 14.0, 16.0], [0.0, 1.0, 1.0, 0.0]))
+    deficit_weight = float(np.interp(
+      deficit, [MACH_E_UNDERSTEER_ERROR_MIN_DEFICIT, MACH_E_UNDERSTEER_ERROR_FULL_DEFICIT], [0.0, 1.0]))
+    return base + (MACH_E_UNDERSTEER_ERROR_MAX - base) * speed_weight * deficit_weight
+
+  def _path_angle_assist(self, requested: float, desired: float, applied: float, v_ego: float,
+                         steering_pressed: bool, lane_change: bool) -> float:
+    target = 0.0
+    if (self.CP.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and self.CP.flags & FordFlags.CANFD and
+        not steering_pressed and not lane_change and 3.0 <= v_ego < MACH_E_PATH_ANGLE_MAX_SPEED and
+        requested * desired > 0.0 and requested * applied > 0.0 and
+        abs(requested) > 0.021 and abs(desired) > 0.021 and abs(applied) >= 0.0195):
+      max_curvature = MAX_LATERAL_ACCEL / v_ego ** 2
+      residual = max(0.0, min(abs(requested), abs(desired), max_curvature) - abs(applied))
+      speed_weight = float(np.interp(
+        v_ego, [MACH_E_PATH_ANGLE_FADE_START_SPEED, MACH_E_PATH_ANGLE_MAX_SPEED], [1.0, 0.0]))
+      target = float(np.sign(applied) * min(residual * v_ego * speed_weight, MACH_E_PATH_ANGLE_MAX))
+    if target == 0.0 or target * self.path_angle_last < 0.0:
+      self.path_angle_last = 0.0
+    else:
+      self.path_angle_last = float(np.clip(
+        target, self.path_angle_last - MACH_E_PATH_ANGLE_STEP, self.path_angle_last + MACH_E_PATH_ANGLE_STEP))
+    return self.path_angle_last
+
   def _blend_and_scale(self, desired: float, predicted: float, v_ego: float, current: float = 0.0,
                        allow_opposite_preview: bool = False) -> tuple[float, int]:
     blend = float(np.interp(abs(desired), [0.0, 0.001], [self.curvature_blend_low, self.curvature_blend_high]))
@@ -436,6 +476,7 @@ class FordLateralController:
       self.manual_turn_direction = 0.0
       self.curvature_samples.clear()
       self.curvature_last = 0.0
+      self.path_angle_last = 0.0
       self.desired_curvature_last = 0.0
       return FordLateralResult()
 
@@ -443,6 +484,7 @@ class FordLateralController:
     if manual_turn or CS.out.vEgoRaw < 0.1:
       self.curvature_samples.clear()
       self.curvature_last = 0.0
+      self.path_angle_last = 0.0
       self.desired_curvature_last = 0.0
       return FordLateralResult(active=not (
         manual_turn and self.CP.carFingerprint in FORD_MANUAL_TURN_LATCH_CARS))
@@ -506,13 +548,16 @@ class FordLateralController:
     self.desired_curvature_last = desired
 
     if v_ego > 9.0:
-      requested = float(np.clip(requested, current - CarControllerParams.CURVATURE_ERROR,
-                                current + CarControllerParams.CURVATURE_ERROR))
+      error_limit = self._curvature_error_limit(
+        requested, desired, current, v_ego, bool(CS.out.steeringPressed), self._lane_change()[0])
+      requested = float(np.clip(requested, current - error_limit, current + error_limit))
     applied = float(apply_std_steer_angle_limits(
       requested, self.curvature_last, v_ego, CS.out.steeringAngleDeg, True, FORD_CURVATURE_LIMITS))
     if self.CP.flags & FordFlags.CANFD:
       max_curvature = MAX_LATERAL_ACCEL / max(v_ego, 1.0) ** 2
       applied = float(np.clip(applied, -max_curvature, max_curvature))
+    path_angle = self._path_angle_assist(
+      requested, desired, applied, v_ego, bool(CS.out.steeringPressed), self._lane_change()[0])
 
     self.curvature_samples.append(predicted)
     curvature_rate = 0.0
@@ -532,6 +577,7 @@ class FordLateralController:
     return FordLateralResult(
       curvature=self.curvature_last,
       curvature_rate=curvature_rate,
+      path_angle=path_angle,
       ramp_type=2,
       precision_type=precision,
       active=True,

starpilot/car/ford/tests/test_lateral.py

@@ -92,6 +92,71 @@ def test_mach_e_unwind_lag_ramps_continuously(controller, monkeypatch):
   assert controller._unwind_preview(0.010, -0.009, 0.011, 10.0) == -0.009
 
 
+@pytest.mark.parametrize("speed,desired,requested,current,driver,lane_change,expected", (
+  (12.0, 0.012, 0.012, 0.004, False, False, 0.006),
+  (12.0, -0.012, -0.012, 0.004, False, False, 0.006),
+  (12.0, 0.012, 0.012, 0.010, False, False, 0.002),
+  (12.0, 0.012, 0.012, 0.014, False, False, 0.002),
+  (9.5, 0.012, 0.012, 0.004, False, False, 0.004),
+  (15.0, 0.012, 0.012, 0.004, False, False, 0.004),
+  (16.0, 0.012, 0.012, 0.004, False, False, 0.002),
+  (12.0, 0.012, 0.012, 0.004, True, False, 0.002),
+  (12.0, 0.012, 0.012, 0.004, False, True, 0.002),
+  (12.0, 0.012, -0.004, 0.004, False, False, 0.002),
+))
+def test_mach_e_understeer_error_scope(controller, speed, desired, requested, current,
+                                      driver, lane_change, expected):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  assert controller._curvature_error_limit(
+    requested, desired, current, speed, driver, lane_change) == pytest.approx(expected)
+
+
+def test_understeer_error_preserves_other_fords(controller):
+  controller.CP.flags = FordFlags.CANFD
+  assert controller._curvature_error_limit(0.012, 0.012, 0.004, 12.0, False, False) == 0.002
+
+
+def test_mach_e_path_angle_assist_at_curvature_limit(controller):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  outputs = [controller._path_angle_assist(0.04, 0.04, 0.02, 7.5, False, False) for _ in range(3)]
+  assert outputs == pytest.approx([0.055, 0.110, 0.150])
+  assert controller._path_angle_assist(0.018, 0.018, 0.018, 7.5, False, False) == 0.0
+  assert controller._path_angle_assist(-0.04, -0.04, -0.02, 7.5, False, False) == pytest.approx(-0.055)
+  assert controller._path_angle_assist(-0.04, -0.04, -0.02, 7.5, True, False) == 0.0
+
+
+@pytest.mark.parametrize("speed,requested,desired,applied,driver,lane_change", (
+  (9.0, 0.04, 0.04, 0.02, False, False),
+  (7.5, 0.020, 0.04, 0.02, False, False),
+  (7.5, 0.04, 0.018, 0.02, False, False),
+  (7.5, 0.04, 0.04, 0.018, False, False),
+  (7.5, 0.04, 0.04, 0.02, True, False),
+  (7.5, 0.04, 0.04, 0.02, False, True),
+))
+def test_mach_e_path_angle_assist_is_scoped(controller, speed, requested, desired, applied, driver, lane_change):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  assert controller._path_angle_assist(requested, desired, applied, speed, driver, lane_change) == 0.0
+
+
+def test_path_angle_assist_preserves_other_fords(controller):
+  controller.CP.flags = FordFlags.CANFD
+  assert controller._path_angle_assist(0.04, 0.04, 0.02, 7.5, False, False) == 0.0
+
+
+def test_mach_e_path_angle_assist_is_encoded_with_curvature(controller):
+  controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1
+  controller.CP.flags = FordFlags.CANFD
+  assist = controller._path_angle_assist(0.04, 0.04, 0.02, 7.5, False, False)
+  packer = CANPacker("ford_lincoln_base_pt")
+  can_bus = CanBus(SimpleNamespace(flags=FordFlags.CANFD, safetyConfigs=[SimpleNamespace()]))
+  _, data, _ = fordcan.create_lat_ctl2_msg(packer, can_bus, 1, 2, 1, -0.02, 0.0, 0, -assist)
+  encoded_angle = (((data[3] & 0x1f) << 6) | (data[4] >> 2)) * 0.0005 - 0.5
+  assert encoded_angle == pytest.approx(-assist)
+
+
 @pytest.mark.parametrize("sign", (-1, 1))
 def test_mach_e_unwind_anticipates_opening_curve_before_current_request_is_met(controller, monkeypatch, sign):
   controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1

starpilot/common/starpilot_variables.py

@@ -1497,8 +1497,6 @@ class StarPilotVariables:
     toggle.startup_alert_bottom = self.get_value("StartupMessageBottom", cast=str, default="")
 
     if toggle.simple_mode:
-      toggle.alert_volume_controller = False
-
       toggle.color_scheme = "stock"
       toggle.current_holiday_theme = "stock"
       toggle.holiday_themes = False

starpilot/navigation/navigationd.py

@@ -203,7 +203,8 @@ class Navigationd:
       "now": now,
     }
 
-  def _maybe_recompute(self, route: NavigationRoute | None, destination: dict[str, object] | None, progress: RouteProgress | None, route_state: dict[str, object] | None) -> None:
+  def _maybe_recompute(self, route: NavigationRoute | None, destination: dict[str, object] | None,
+                       progress: RouteProgress | None, route_state: dict[str, object] | None) -> None:
     if route is None or destination is None or progress is None or route_state is None:
       return
 
@@ -301,6 +302,7 @@ class Navigationd:
 
     state = {
       "valid": True,
+      "updatedAtMonotonic": monotonic(),
       "maneuverModifier": str(payload.get("maneuverModifier") or ""),
       "maneuverType": str(payload.get("maneuverType") or ""),
       "laneCount": len(lanes),

starpilot/navigation/test_navigationd.py

@@ -70,3 +70,20 @@ def test_run_builds_one_payload_for_both_navigation_publishers():
   instruction_payload = navigationd._publish_nav_instruction.calls[0][3]
   state_payload = navigationd._publish_nav_state.calls[0][3]
   assert instruction_payload is state_payload
+
+
+def test_navigation_state_timestamp_refreshes_even_when_instruction_is_unchanged():
+  navigationd = Navigationd.__new__(Navigationd)
+  navigationd._last_nav_state = None
+  navigationd.params_memory = type("Memory", (), {})()
+  navigationd.params_memory.put_nonblocking = Recorder()
+  navigationd.params_memory.remove = Recorder()
+  payload = {"maneuverType": "turn", "maneuverModifier": "right", "maneuverDistance": 55.0}
+
+  navigationd._publish_nav_state(object(), object(), True, payload)
+  navigationd._publish_nav_state(object(), object(), True, payload)
+
+  states = [args[1] for args in navigationd.params_memory.put_nonblocking.calls]
+  assert len(states) == 2
+  assert all(state["valid"] and state["updatedAtMonotonic"] > 0 for state in states)
+  assert states[0]["updatedAtMonotonic"] < states[1]["updatedAtMonotonic"]

starpilot/system/the_galaxy/the_galaxy.py

@@ -6724,6 +6724,12 @@ def setup(app):
       if key == "RivianAngleControl":
         response["message"] = "Rivian steering mode updated. The safe channel handoff is in progress."
       updated = {}
+      if key in {
+        "BelowSteerSpeedVolume", "DisengageVolume", "EngageVolume", "PromptVolume",
+        "PromptDistractedVolume", "RefuseVolume", "WarningImmediateVolume", "WarningSoftVolume",
+      }:
+        _, value_types = _get_param_type_info()
+        updated[key] = _get_current_param_value(key, value_types.get(key, int), _get_default_param_values())
       if key in PANDA_FIRMWARE_TOGGLE_KEYS:
         threading.Thread(target=_flash_panda_then_reboot, daemon=True).start()
         response["message"] = f"Parameter '{key}' updated successfully. Panda flashing started; device will reboot when finished."

tools/replay/fake_nav_demo.py

@@ -113,6 +113,7 @@ SCENARIOS = [
 def build_nav_state(params_memory: Params, scenario: dict) -> None:
   params_memory.put_nonblocking("NavInstructionState", {
     "valid": True,
+    "updatedAtMonotonic": time.monotonic(),
     "maneuverModifier": str(scenario["modifier"]),
     "maneuverType": str(scenario["type"]),
     "maneuverPrimaryText": str(scenario["primary"]),