Match StarPilot Ford demand to native pinion bounds

openpilot/starpilot/car/ford/bluepilot.py

@@ -18,6 +18,8 @@ from opendbc.bluepilot_lateral.core.lateral_inputs import lateral_inputs_complet
 from opendbc.bluepilot_lateral.can.fordcan_ext import lat_ctl2_checksum, create_lat_ctl2_msg
 from opendbc.car.ford.values import CarControllerParams, FordFlags
 from opendbc.car import structs
+from opendbc.car.vehicle_model import calc_slip_factor
+from opendbc.bluepilot_lateral.core.carcontroller_ext import pinion_vehicle_model
 from opendbc.car.lateral import ISO_LATERAL_ACCEL
 from openpilot.starpilot.car.ford.controller_selection import FordController, read_controller
 
@@ -137,10 +139,13 @@ class FordBluePilotController(BluePilotLateral):
     self.measured_history = deque([0] * 6, maxlen=6)
     self.measured_speed = 0
     self.measured_seen = False
+    self.native_pinion_model = pinion_vehicle_model(CP) if self.inputs.bp_pinion_curvature_enabled else None
+    self.native_pinion_slip = (np.float32(calc_slip_factor(self.native_pinion_model))
+                               if self.native_pinion_model is not None else np.float32(0.))
 
   def observe_can(self, can_list):
     # Match Panda's receive order: yaw can precede the new speed in the same batch.
-    if not self.CP.flags & FordFlags.CANFD and not self.starpilot_selected:
+    if not self.CP.flags & FordFlags.CANFD and not self.starpilot_selected and self.native_pinion_model is None:
       return
     for _, batch in can_list:
       for address, data, bus in batch:
@@ -150,6 +155,18 @@ class FordBluePilotController(BluePilotLateral):
           speed = np.float32(((data[0] << 8) | data[1]) * .01 * (1. / 3.6))
           self.measured_speed = math.floor(float(np.float32(speed * 1000.)) + .5)
           self.speed_history.append(self.measured_speed)
+        elif address == 0x7E and self.native_pinion_model is not None:
+          # Mirror the fixed native pinion geometry, before live ratio/roll corrections.
+          raw_angle = ((data[2] & 0x7F) << 8) | data[3]
+          angle = np.float32(np.float32(raw_angle) * np.float32(.1) - np.float32(1600.))
+          radians = np.float32(angle * np.float32(math.pi / 180.))
+          speed = np.float32(max(self.measured_speed / 1000., .1))
+          slip_speed = np.float32(self.native_pinion_slip * np.float32(speed * speed))
+          factor = np.float32(1. / (1. - float(slip_speed)) / float(np.float32(self.native_pinion_model.l)))
+          curvature = np.float32(np.float32(radians * factor) / np.float32(self.native_pinion_model.sR))
+          raw = float(np.float32(curvature * np.float32(50000.)))
+          self.measured_history.append(math.floor(raw + .5) if raw >= 0 else math.ceil(raw - .5))
+          self.measured_seen = True
         elif address == 0x91 and (data[6] >> 4) & 3 == 3 and not self.inputs.bp_pinion_curvature_enabled:
           yaw = np.float32(((data[2] << 8) | data[3]) * .0002 - 6.5)
           speed = np.float32(self.measured_speed / 1000.)
@@ -185,6 +202,16 @@ class FordBluePilotController(BluePilotLateral):
     self.lateral_active = bool(CC.latActive)
     if self.starpilot_selected and (not self.starpilot_frame_valid or not self.angle.inputs_valid(CS, actuators)):
       CC = structs.CarControl.new_message(latActive=False)
+    if self.starpilot_selected:
+      self.angle.native_curvature_bounds = None
+      if self.native_pinion_model is not None and self.measured_seen and CS.out.vEgoRaw > 10.:
+        # Pinion native limits use 150 CAN curvature units, without learned geometry.
+        error = 150 * .00002
+        low = max(-max(self.measured_history) * .00002 - error, measured - CarControllerParams.CURVATURE_ERROR)
+        high = min(-min(self.measured_history) * .00002 + error, measured + CarControllerParams.CURVATURE_ERROR)
+        # Tighten the host envelope only where both measurements agree on a command.
+        if low <= high:
+          self.angle.native_curvature_bounds = (low, high)
     packets, applied = super().step_lateral(CC, CS, actuators, apply_curvature_last)
     return self._constrain_curvature(CC, CS, actuators, apply_curvature_last, packets, applied)
 

openpilot/starpilot/car/ford/starpilot_controller.py

@@ -103,6 +103,7 @@ class StarPilotFordController:
   def __init__(self, CP, inputs, shared):
     self.CP = CP
     self.conditioner = CurvatureConditioner()
+    self.native_curvature_bounds = None
     # StarPilot uses the platform defaults, not hidden BluePilot user adjustment knobs.
     for attr, key in ANGLE_SETTINGS:
       setattr(self, attr, default(key))
@@ -337,6 +338,12 @@ class StarPilotFordController:
       # not rate-of-change)? The BluePilot UI's deviation flag.
       self.shared.bp_curvature_deviation_limited = bool(abs(kappa_cmd - _kappa_cmd_pre_error_clip) > 1e-9)
 
+    if self.native_curvature_bounds is not None:
+      low, high = self.native_curvature_bounds
+      constrained = float(clip(kappa_cmd, low, high))
+      self.shared.bp_curvature_deviation_limited |= abs(constrained - kappa_cmd) > 1e-9
+      kappa_cmd = constrained
+
     # Remember only demand admitted by the measured-curvature envelope, not a hidden backlog.
     self.conditioner.curvature = kappa_cmd
     if CS.out.steeringPressed:

openpilot/starpilot/tests/test_ford_native_pinion_guard.py

@@ -0,0 +1,104 @@
+"""StarPilot demand uses the native pinion envelope before path-angle conversion."""
+import math
+
+import pytest
+
+from opendbc.can import CANPacker
+from opendbc.car import gen_empty_fingerprint, structs
+from opendbc.car.ford.carcontroller import CarController
+from opendbc.car.ford.carstate import CarState
+from opendbc.car.ford.interface import CarInterface
+from opendbc.car.ford.tests.test_bluepilot_lateral import StraightRoadInputs, STARTUP, settings
+from opendbc.car.ford.values import CAR, DBC, CarControllerParams
+from opendbc.bluepilot_lateral.hosts.starpilot.safety_param import init_ford_safety_param_starpilot
+from opendbc.safety.tests.libsafety import libsafety_py
+from openpilot.starpilot.car.ford.bluepilot import FordBluePilotController
+from openpilot.starpilot.car.ford.controller_selection import FordController
+
+
+class LearnedGeometryInputs(StraightRoadInputs):
+  controller_choice = FordController.STARPILOT
+  current_inputs_valid = True
+
+  def __init__(self, cp, ratio, roll):
+    super().__init__(settings(STARTUP['on+pinion'], True))
+    self.cp, self.ratio, self.roll = cp, ratio, roll
+
+  def update(self):
+    frame = super().update()
+    frame.lateralInputs.steerRatio = self.cp.steerRatio * self.ratio
+    frame.lateralInputs.roll = self.roll
+    return frame
+
+
+@pytest.mark.parametrize('car', [CAR.FORD_MUSTANG_MACH_E_MK1, CAR.FORD_EXPLORER_MK6, CAR.FORD_F_150_MK14])
+@pytest.mark.parametrize('speed', [15., 30.])
+@pytest.mark.parametrize('sign', [-1., 1.])
+@pytest.mark.parametrize('ratio, refused', [(.85, False), (.5, True)])
+def test_pinion_guard_preserves_host_and_native_bounds(car, speed, sign, ratio, refused):
+  fp = gen_empty_fingerprint()
+  fp[0][0x5A] = 8
+  fp[2].update({0x3D6: 8, 0x186: 8})
+  cp = CarInterface.get_params(car, fp, [], False, False, False)
+  init_ford_safety_param_starpilot(cp, STARTUP['on+pinion'])
+  controller = CarController(DBC[cp.carFingerprint], cp)
+  inputs = LearnedGeometryInputs(cp, ratio, -sign * .04)
+  controller.bluepilot_inputs = inputs
+  controller.bp = FordBluePilotController(cp, controller.packer, controller.CAN, inputs)
+  state = CarState(cp)
+  state.update(state.get_can_parsers(cp))
+  safety, packer = libsafety_py.libsafety, CANPacker('ford_lincoln_base_pt')
+  safety.init_tests()
+  safety.set_alternative_experience(0)
+  assert safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, cp.safetyConfigs[-1].safetyParam) == 0
+  safety.set_controls_allowed(True)
+  blocked, sent = [], 0
+  try:
+    for frame in range(100):
+      safety.set_timer(1_000_000 + frame * 10_000)
+      frames = []
+      def rx(name, values, frames=frames):
+        address, data, bus = packer.make_can_msg(name, 0, values)
+        assert safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data))
+        frames.append((address, data, bus))
+      if frame % 2 == 0:
+        rx('BrakeSysFeatures', {'Veh_V_ActlBrk': speed * 3.6, 'VehVActlBrk_D_Qf': 3,
+                                'VehVActlBrk_No_Cnt': frame // 2 % 16})
+        rx('EngVehicleSpThrottle2', {'Veh_V_ActlEng': speed * 3.6, 'VehVActlEng_D_Qf': 3})
+        rx('DesiredTorqBrk', {'VehStop_D_Stat': 0})
+      rx('Yaw_Data_FD1', {'VehYaw_W_Actl': 0., 'VehYawWActl_D_Qf': 3, 'VehRollYaw_No_Cnt': frame % 256})
+      rx('EngVehicleSpThrottle', {'ApedPos_Pc_ActlArb': 0})
+      if frame % 10 == 0:
+        rx('EngBrakeData', {'BpedDrvAppl_D_Actl': 1, 'CcStat_D_Actl': 5})
+        rx('Steering_Data_FD1', {})
+      rx('SteeringPinion_Data', {'StePinComp_An_Est': sign * 30., 'StePinCompAnEst_D_Qf': 3,
+                                'StePinAn_No_Cnt': frame % 16})
+      controller.bp.observe_can([(0, frames)])
+      if frame >= 6 and hasattr(controller.bp, 'native_pinion_model'):
+        assert min(controller.bp.measured_history) == safety.get_curvature_meas_min()
+        assert max(controller.bp.measured_history) == safety.get_curvature_meas_max()
+      cs = structs.CarState(vEgo=speed, vEgoRaw=speed, steeringAngleDeg=sign * 30., canValid=True,
+                            gearShifter=structs.CarState.GearShifter.drive)
+      cs.cruiseState.available = cs.cruiseState.enabled = True
+      state.out = cs.as_reader()
+      cc = structs.CarControl(enabled=True, latActive=True)
+      cc.actuators.curvature = -sign * .02
+      _, packets = controller.update(cc.as_reader(), state, (1_000_000 + frame * 10_000) * 1000)
+      for address, data, bus in packets:
+        if address in (0x3CA, 0x3D3, 0x3D6):
+          sent += 1
+          if not safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)):
+            blocked.append((frame, hex(address)))
+    assert sent > 40
+    if refused:
+      assert blocked
+      assert controller.bp.angle.native_curvature_bounds is None
+    else:
+      assert not blocked, blocked
+      low, high = controller.bp.angle.native_curvature_bounds
+      assert low <= controller.bp.angle.bp_kappa_cmd <= high
+      measured = controller.bp.inputs.get_current_curvature(state)
+      assert abs(controller.bp.angle.bp_kappa_cmd - measured) <= CarControllerParams.CURVATURE_ERROR + 1e-12
+    assert math.isfinite(controller.bp.angle.bp_path_angle_final)
+  finally:
+    safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0)

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "fcb65e816e68be49bb32fb439c412fb748d2d9d7facc1aa0e8ec50df76a6c685", "schema_version": 1}
+{"protected_inventory_sha256": "eb1624c8c953b7e84b7fd952465e7a153f5de9959bfc4d2a79ec426f473b27fc", "schema_version": 1}