diff --git a/openpilot/starpilot/car/ford/bluepilot.py b/openpilot/starpilot/car/ford/bluepilot.py index e90ac4cafb..c4d1ef0f4f 100644 --- a/openpilot/starpilot/car/ford/bluepilot.py +++ b/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) diff --git a/openpilot/starpilot/car/ford/starpilot_controller.py b/openpilot/starpilot/car/ford/starpilot_controller.py index 0d8079357c..8db1093447 100644 --- a/openpilot/starpilot/car/ford/starpilot_controller.py +++ b/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: diff --git a/openpilot/starpilot/tests/test_ford_native_pinion_guard.py b/openpilot/starpilot/tests/test_ford_native_pinion_guard.py new file mode 100644 index 0000000000..5404a9235f --- /dev/null +++ b/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) diff --git a/prebuilt.json b/prebuilt.json index 91692c08d1..8005d73bd2 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "fcb65e816e68be49bb32fb439c412fb748d2d9d7facc1aa0e8ec50df76a6c685", "schema_version": 1} +{"protected_inventory_sha256": "eb1624c8c953b7e84b7fd952465e7a153f5de9959bfc4d2a79ec426f473b27fc", "schema_version": 1}