mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-28 17:13:54 +08:00
Ford: add opt-in pinion curvature measurement
Adapt BluePilot PR #145 by jrneulight (fe5b019568c8c13fb894dc6d8d779d60985051a8) to the existing exact native profile protocol. Default off; qualified parked setting applies at next startup. Preserve existing larger Mach-E envelope, all yaw/heartbeat/relay health, command limits and shadow freshness checks. Cloud qualification: 171 host tests and 170 subtests; 80 new native/geometry tests; 153 existing native tests and 9131 subtests; release native tests, pandad protocol inventory and full ARM debug/release builds passed. Rebuilt source-stamped firmware follows before branch publication.
This commit is contained in:
Dom
@@ -33,3 +33,7 @@ Spotify's Bluetooth output binds the comma's BlueALSA A2DP source to the selecte
|
||||
## Tesla Pre-AP pedal control
|
||||
|
||||
The Pre-AP configuration, pedal codec/feedback, VirtualDAS feedforward table, grade compensation, torque-zero estimator and vehicle-specific gains preserve firestar5683's intentional Dom work, including `7837a3c1aab`, `aed0b7fdb9e9` and pinned `b220a37f4bd119e402225a7aa16a7cd84878d0af`. Original author attribution: firestar5683 <168790843+firestar5683@users.noreply.github.com>. Startup/calibration ownership, current upstream button/PID interfaces and native command bounds are adapted to the current StarPilot architecture; the old permissive native gateway is not transplanted. Existing source/license notices remain intact.
|
||||
|
||||
## Ford optional pinion measurement
|
||||
|
||||
The default-off Use Pinion Yaw Sensor measurement option is adapted from jrneulight’s BluePilot PR #145 (https://github.com/BluePilotDev/bluepilot/pull/145), merged as `fe5b019568c8c13fb894dc6d8d779d60985051a8`. Its per-platform geometry, native pinion measurement, integrity requirements and host vehicle-model compensation are retained. StarPilot adapts the option to its existing exact safety-profile word, modern curvature checks, AOL authority and Galaxy/native settings; the upstream maintainers do not maintain this integration. Existing upstream copyright and license notices remain in THIRD_PARTY_NOTICES.md.
|
||||
|
||||
@@ -4,6 +4,7 @@ Only exact final CP words derived from the existing five classic/six CANFD
|
||||
extended profiles can select angle. Combined AOL retains its original Ford profile identity.
|
||||
"""
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.ford.pinion import base_word, configured, MASK as PINION_MASK
|
||||
from opendbc.car.ford.values import CAR, FordFlags
|
||||
from opendbc.bluepilot_lateral.core.angle import AngleController, Inputs
|
||||
from opendbc.bluepilot_lateral.core.trim import LaneGeometry
|
||||
@@ -39,7 +40,9 @@ def mode_word(cp, *, selected):
|
||||
expected = original | (256 if manual else 0) | (ANGLE_FLAG if selected else 0)
|
||||
else:
|
||||
expected = ANGLE_FLAG + (2 if fd else 0) + long if selected else original
|
||||
return expected if safety.safetyModel == structs.CarParams.SafetyModel.ford and safety.safetyParam == expected else None
|
||||
valid = (safety.safetyModel == structs.CarParams.SafetyModel.ford and base_word(safety.safetyParam) == expected and
|
||||
(safety.safetyParam == expected or configured(cp)))
|
||||
return expected if valid else None
|
||||
|
||||
|
||||
def supported(cp):
|
||||
@@ -58,7 +61,8 @@ def select(cp, raw):
|
||||
if cp.alternativeExperience == 32:
|
||||
cp.safetyConfigs[-1].safetyParam |= ANGLE_FLAG
|
||||
else:
|
||||
cp.safetyConfigs[-1].safetyParam = ANGLE_FLAG + (2 if cp.flags & FordFlags.CANFD else 0) + int(cp.openpilotLongitudinalControl)
|
||||
metadata = cp.safetyConfigs[-1].safetyParam & PINION_MASK
|
||||
cp.safetyConfigs[-1].safetyParam = (ANGLE_FLAG + (2 if cp.flags & FordFlags.CANFD else 0) + int(cp.openpilotLongitudinalControl)) | metadata
|
||||
cp.steerActuatorDelay = 0.22
|
||||
return True
|
||||
|
||||
@@ -86,6 +90,8 @@ class Host:
|
||||
raise ValueError("BluePilot angle requires an exact admitted Ford safety word")
|
||||
low, high = (0.95, 0.95) if cp.carFingerprint in TRUCKS else ((1.0, 1.05) if cp.carFingerprint in CANFD else (1.0, 1.15))
|
||||
self.cp = cp
|
||||
from opendbc.car.ford.pinion import Measurement
|
||||
self.measurement = Measurement(cp)
|
||||
self.controller = AngleController(low_gain=low, high_gain=high)
|
||||
self.inputs = None
|
||||
self.steering_rejected = False
|
||||
@@ -93,6 +99,7 @@ class Host:
|
||||
def update(self, CC, CS):
|
||||
if self.inputs is not None:
|
||||
self.inputs.update()
|
||||
self.measurement.update_parameters(getattr(self.inputs, "vehicle_parameters", None))
|
||||
snapshot = self.inputs.lateral_snapshot(CS.out.vEgoRaw, angle=True) if self.inputs is not None else None
|
||||
model, times, delay, _ = snapshot if snapshot is not None else (None, (), 0.15, True)
|
||||
aol = self.cp.alternativeExperience == 32
|
||||
@@ -121,7 +128,7 @@ class Host:
|
||||
speed_max = max(high, CS.out.vEgoRaw, 0.1) + margin
|
||||
meta = model.meta if model is not None else None
|
||||
return self.controller.step(Inputs(
|
||||
active=ready, speed=CS.out.vEgoRaw, measured_curvature=-CS.out.yawRate / max(CS.out.vEgoRaw, 0.1),
|
||||
active=ready, speed=CS.out.vEgoRaw, measured_curvature=self.measurement.curvature(CS.out),
|
||||
desired_curvature=CC.actuators.curvature, steering_pressed=CS.out.steeringPressed,
|
||||
steering_angle=CS.out.steeringAngleDeg, yaw_predictions=tuple(model.orientationRate.z) if model is not None else (),
|
||||
time_indices=tuple(times), lateral_delay=delay,
|
||||
|
||||
@@ -213,8 +213,8 @@ static bool ford_bp_lateral(const CANPacket_t *msg) {
|
||||
(SAFETY_ABS(path) >= (relation_low - 2.0F)) && (SAFETY_ABS(path) <= (relation_high + 2.0F)) &&
|
||||
(SAFETY_ABS(shadow) <= (physical_cap + FORD_BP_SHADOW_ROUNDING_MARGIN)) && (path_curvature_high <= (physical_cap + FORD_BP_SHADOW_ROUNDING_MARGIN));
|
||||
if (speed_max > 10.0F) {
|
||||
const int measured_min = curvature_state.meas.min - 100;
|
||||
const int measured_max = curvature_state.meas.max + 100;
|
||||
const int measured_min = curvature_state.meas.min - (ford_pinion_enabled ? 150 : 100);
|
||||
const int measured_max = curvature_state.meas.max + (ford_pinion_enabled ? 150 : 100);
|
||||
const float measured_min_float = (float)measured_min;
|
||||
const float measured_max_float = (float)measured_max;
|
||||
valid &= (shadow_can >= measured_min_float) && (shadow_can <= measured_max_float);
|
||||
|
||||
@@ -12,6 +12,9 @@ ANGLE = 128
|
||||
ANGLE_BASE_WORDS = frozenset((18, 19, 32, 33, 66, 67))
|
||||
AOL_WORDS = (BASE_WORDS | frozenset(word | ANGLE for word in ANGLE_BASE_WORDS))
|
||||
AOL_WORDS |= frozenset(word | MANUAL for word in AOL_WORDS)
|
||||
from opendbc.car.ford.pinion import ENABLE, SHIFT, PROFILES, base_word, configured
|
||||
AOL_WORDS |= frozenset(word | ENABLE | index << SHIFT for index, words in PROFILES.items()
|
||||
for word in tuple(AOL_WORDS) if (word & ~MANUAL) in words)
|
||||
|
||||
|
||||
def aol_safety_config(cp):
|
||||
@@ -33,6 +36,10 @@ def base_safety_param(cp):
|
||||
if cp.transmissionType not in (structs.CarParams.TransmissionType.automatic, structs.CarParams.TransmissionType.manual):
|
||||
return -1
|
||||
word = int(owner.safetyParam)
|
||||
normalized = base_word(word)
|
||||
if normalized != word and not configured(cp):
|
||||
return -1
|
||||
word = normalized
|
||||
if (word & MANUAL and not manual) or (manual and cp.alternativeExperience == AOL_EXPERIENCE and not word & MANUAL):
|
||||
return -1
|
||||
if word & ANGLE:
|
||||
|
||||
@@ -183,7 +183,7 @@ class CarController(CarControllerBase):
|
||||
demanded = self.classic_lateral.update(CC, CS, actuators) if self.classic_extended_announced or self.generic_canfd else FordLateralResult()
|
||||
bound = generic_canfd_bounded_command if self.generic_canfd else classic_bounded_command
|
||||
lateral = bound(self.classic_lateral, demanded, previous, CS.out.vEgoRaw,
|
||||
-CS.out.yawRate / max(CS.out.vEgoRaw, 0.1))
|
||||
self.classic_lateral._current_curvature(CS))
|
||||
self.classic_lateral_demand = demanded
|
||||
self.apply_curvature_last = lateral.curvature
|
||||
if self.generic_canfd:
|
||||
@@ -209,7 +209,7 @@ class CarController(CarControllerBase):
|
||||
previous = self.new_port_curvature.curvature_last
|
||||
demanded = self.new_port_curvature.update(CC, CS, actuators)
|
||||
lateral = new_port_bounded_command(self.new_port_curvature, demanded, previous, CS.out.vEgoRaw,
|
||||
-CS.out.yawRate / max(CS.out.vEgoRaw, 0.1))
|
||||
self.new_port_curvature._current_curvature(CS))
|
||||
self.new_port_curvature_demand = demanded
|
||||
self.apply_curvature_last = lateral.curvature
|
||||
if self.CP.flags & FordFlags.CANFD:
|
||||
|
||||
@@ -162,6 +162,8 @@ class FordLateralController:
|
||||
|
||||
def __init__(self, CP):
|
||||
self.CP = CP
|
||||
from opendbc.car.ford.pinion import Measurement
|
||||
self.measurement = Measurement(CP)
|
||||
self.model = None
|
||||
self.time_indices = ()
|
||||
self.lookahead = CURVATURE_LOOKAHEAD_MIN
|
||||
@@ -231,9 +233,8 @@ class FordLateralController:
|
||||
return MACH_E_HIGH_SPEED_LOOKAHEAD_EXTRA * float(np.interp(
|
||||
v_ego, [MACH_E_HIGH_SPEED_LOOKAHEAD_START_SPEED, MACH_E_HIGH_SPEED_LOOKAHEAD_FULL_SPEED], [0.0, 1.0]))
|
||||
|
||||
@staticmethod
|
||||
def _current_curvature(CS) -> float:
|
||||
return -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
|
||||
def _current_curvature(self, CS) -> float:
|
||||
return self.measurement.curvature(CS.out)
|
||||
|
||||
def _driver_assisting_curve(self, CS, desired: float) -> bool:
|
||||
v_ego = float(CS.out.vEgoRaw)
|
||||
|
||||
@@ -0,0 +1,84 @@
|
||||
"""Init-only pinion measurement selection adapted from BluePilot PR #145.
|
||||
|
||||
See CREDITS.md and THIRD_PARTY_NOTICES.md for upstream attribution and license.
|
||||
The geometry is carried in the existing exact Panda safety word, not live Params.
|
||||
"""
|
||||
from opendbc.car.ford.values import CAR
|
||||
|
||||
KEY = "FordPrefSteerAngleCurvature"
|
||||
ENABLE = 0x200
|
||||
SHIFT = 10
|
||||
MASK = 0x3E00
|
||||
GEOMETRY = {
|
||||
CAR.FORD_BRONCO_SPORT_MK1: 1, CAR.FORD_ESCAPE_MK4: 2, CAR.FORD_ESCAPE_MK4_5: 3,
|
||||
CAR.FORD_EXPEDITION_MK4: 4, CAR.FORD_EXPLORER_MK6: 5, CAR.FORD_FOCUS_MK4: 6,
|
||||
CAR.FORD_F_150_LIGHTNING_MK1: 7, CAR.FORD_F_150_MK14: 8, CAR.FORD_MAVERICK_MK1: 9,
|
||||
CAR.FORD_MONDEO_MK5: 10, CAR.FORD_MUSTANG_MACH_E_MK1: 11, CAR.FORD_RANGER_MK2: 12,
|
||||
}
|
||||
CLASSIC = frozenset((32, 33, 128, 129, 160, 161))
|
||||
GENERIC = frozenset((66, 67, 130, 131, 194, 195))
|
||||
MACH_E = frozenset((18, 19, 130, 131, 146, 147))
|
||||
PROFILES = {i: CLASSIC if i in (1, 2, 5, 6, 9) else MACH_E if i == 11 else frozenset((10, 11)) if i == 10 else GENERIC
|
||||
for i in range(1, 13)}
|
||||
|
||||
|
||||
def decode(word):
|
||||
"""Return (original profile, geometry), or None for malformed metadata."""
|
||||
if not word & MASK:
|
||||
return word, 0
|
||||
index = (word >> SHIFT) & 15
|
||||
base = word & ~MASK
|
||||
if not word & ENABLE or index not in PROFILES or (base & ~256) not in PROFILES[index]:
|
||||
return None
|
||||
return base, index
|
||||
|
||||
|
||||
def base_word(word):
|
||||
result = decode(int(word))
|
||||
return result[0] if result is not None else -1
|
||||
|
||||
|
||||
def configured(cp):
|
||||
if cp.brand != "ford" or not cp.safetyConfigs:
|
||||
return False
|
||||
result = decode(int(cp.safetyConfigs[-1].safetyParam))
|
||||
return bool(result is not None and result[1] and result[1] == GEOMETRY.get(cp.carFingerprint))
|
||||
|
||||
|
||||
def select(cp, enabled):
|
||||
if not enabled or cp.brand != "ford" or cp.passive or cp.dashcamOnly or cp.notCar:
|
||||
return False
|
||||
from opendbc.car.ford.aol import qualified
|
||||
if not qualified(cp):
|
||||
return False
|
||||
index = GEOMETRY.get(cp.carFingerprint)
|
||||
if index is None:
|
||||
return False
|
||||
cp.safetyConfigs[-1].safetyParam |= ENABLE | index << SHIFT
|
||||
return True
|
||||
|
||||
|
||||
class Measurement:
|
||||
"""Same init-time family as native; live model correction never changes the source."""
|
||||
def __init__(self, cp):
|
||||
self.enabled = configured(cp)
|
||||
self.offset = self.roll = 0.0
|
||||
self.model = None
|
||||
if self.enabled:
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
self.model = VehicleModel(cp)
|
||||
|
||||
def update_parameters(self, parameters):
|
||||
import math
|
||||
if self.model is None or parameters is None:
|
||||
return
|
||||
values = (parameters.angleOffsetDeg, parameters.roll, parameters.stiffnessFactor, parameters.steerRatio)
|
||||
if parameters.valid and all(math.isfinite(v) for v in values):
|
||||
self.offset, self.roll = values[:2]
|
||||
self.model.update_params(max(values[2], 0.1), max(values[3], 0.1))
|
||||
|
||||
def curvature(self, state):
|
||||
if not self.enabled:
|
||||
return -state.yawRate / max(state.vEgoRaw, 0.1)
|
||||
import math
|
||||
return -self.model.calc_curvature(math.radians(state.steeringAngleDeg - self.offset), state.vEgoRaw, self.roll)
|
||||
@@ -0,0 +1,157 @@
|
||||
"""Closed, init-only measurement selection; disabled behavior stays unchanged."""
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
from opendbc.car import gen_empty_fingerprint, structs
|
||||
from opendbc.car.ford.interface import CarInterface
|
||||
from opendbc.car.ford.pinion import GEOMETRY, ENABLE, SHIFT, MASK, decode, select, configured, Measurement
|
||||
from opendbc.car.ford.aol import qualified
|
||||
from opendbc.car.vehicle_model import VehicleModel, calc_slip_factor
|
||||
from opendbc.can.packer import CANPacker
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
|
||||
|
||||
def params(car, long=False):
|
||||
fp = gen_empty_fingerprint()
|
||||
fp[0][0x5A] = 8
|
||||
fp[2].update({0x3D6: 8, 0x186: 8})
|
||||
return CarInterface.get_params(car, fp, [], long, False, False)
|
||||
|
||||
|
||||
def test_closed_decoder_matches_native_exhaustively():
|
||||
n = libsafety_py.libsafety
|
||||
for word in range(65536):
|
||||
decoded = decode(word)
|
||||
assert n.get_ford_pinion_base(word) == (decoded[0] if decoded is not None else 65535)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('car', list(GEOMETRY))
|
||||
@pytest.mark.parametrize('long', [False, True])
|
||||
def test_default_off_and_exact_geometry_selection(car, long):
|
||||
cp = params(car, long)
|
||||
before = cp.to_dict()
|
||||
assert not select(cp, False)
|
||||
assert cp.to_dict() == before
|
||||
assert select(cp, True)
|
||||
assert configured(cp) and qualified(cp)
|
||||
assert decode(cp.safetyConfigs[-1].safetyParam)[1] == GEOMETRY[car]
|
||||
# Cross-family/identity corruption is not accepted by the host.
|
||||
cp.safetyConfigs[-1].safetyParam = (cp.safetyConfigs[-1].safetyParam & ~MASK) | ENABLE | ((GEOMETRY[car] % 12 + 1) << SHIFT)
|
||||
assert not configured(cp) and not qualified(cp)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('car', list(GEOMETRY))
|
||||
def test_geometry_matches_actual_vehicle_model_and_native_sign(car):
|
||||
cp = params(car)
|
||||
index = GEOMETRY[car]
|
||||
n = libsafety_py.libsafety
|
||||
vm = VehicleModel(cp)
|
||||
assert n.get_ford_pinion_slip(index) == pytest.approx(calc_slip_factor(vm), abs=2e-10)
|
||||
assert n.get_ford_pinion_ratio(index) == pytest.approx(cp.steerRatio)
|
||||
assert n.get_ford_pinion_wheelbase(index) == pytest.approx(cp.wheelbase)
|
||||
assert select(cp, True)
|
||||
host = Measurement(cp)
|
||||
packer = CANPacker('ford_lincoln_base_pt')
|
||||
for speed in (0., 5., 15., 30.):
|
||||
for angle in (-100., -10., 0., 10., 100.):
|
||||
n.set_alternative_experience(0)
|
||||
n.set_safety_hooks(structs.CarParams.SafetyModel.ford, cp.safetyConfigs[-1].safetyParam)
|
||||
assert n.get_ford_pinion_enabled()
|
||||
for counter in range(6):
|
||||
a, d, b = packer.make_can_msg('BrakeSysFeatures', 0, {'Veh_V_ActlBrk': speed * 3.6, 'VehVActlBrk_D_Qf': 3})
|
||||
assert n.safety_rx_hook(libsafety_py.make_CANPacket(a, b, d))
|
||||
a, d, b = packer.make_can_msg('SteeringPinion_Data', 0, {'StePinComp_An_Est': angle, 'StePinCompAnEst_D_Qf': 3, 'StePinAn_No_Cnt': counter})
|
||||
assert n.safety_rx_hook(libsafety_py.make_CANPacket(a, b, d))
|
||||
state = SimpleNamespace(vEgoRaw=max(speed, .1), yawRate=99., steeringAngleDeg=angle)
|
||||
expected = -host.curvature(state) * 50000
|
||||
assert n.get_curvature_meas_min() == pytest.approx(expected, abs=1.)
|
||||
assert n.get_curvature_meas_max() == pytest.approx(expected, abs=1.)
|
||||
# A valid but contradictory yaw packet never changes selected pinion measurement.
|
||||
a, d, b = packer.make_can_msg('Yaw_Data_FD1', 0, {'VehYaw_W_Actl': 1., 'VehYawWActl_D_Qf': 3})
|
||||
assert n.safety_rx_hook(libsafety_py.make_CANPacket(a, b, d))
|
||||
assert n.get_curvature_meas_min() == pytest.approx(expected, abs=1.)
|
||||
|
||||
|
||||
def test_default_measurement_is_yaw_even_with_conflicting_pinion():
|
||||
cp = params(next(iter(GEOMETRY)))
|
||||
m = Measurement(cp)
|
||||
state = SimpleNamespace(vEgoRaw=15., yawRate=.3, steeringAngleDeg=100.)
|
||||
assert m.curvature(state) == -.3 / 15.
|
||||
assert not m.enabled
|
||||
|
||||
|
||||
@pytest.mark.parametrize('index,base', [(1, 32), (3, 66), (10, 10), (11, 18)])
|
||||
@pytest.mark.parametrize('long', [False, True])
|
||||
@pytest.mark.parametrize('manual', [False, True])
|
||||
def test_pinion_inventory_and_invalid_quality_withdraw_authority(index, base, long, manual):
|
||||
from opendbc.safety.tests.test_ford_aol import TestFordAolDriverIntent
|
||||
n = TestFordAolDriverIntent()
|
||||
n.setUp()
|
||||
base |= int(long) | (256 if manual else 0)
|
||||
n.reset(base)
|
||||
count = n.safety.get_ford_pinion_rx_count()
|
||||
n.safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, base | ENABLE | index << SHIFT)
|
||||
n.safety.set_aol_test_heartbeat(True)
|
||||
assert n.safety.get_ford_pinion_rx_count() == count + 1
|
||||
original_rx = n.rx
|
||||
|
||||
def rx(name, values):
|
||||
if name == 'Yaw_Data_FD1':
|
||||
original_rx('SteeringPinion_Data', {'StePinComp_An_Est': 0., 'StePinCompAnEst_D_Qf': 3, 'StePinAn_No_Cnt': n.tick % 16})
|
||||
original_rx(name, values)
|
||||
n.rx = rx
|
||||
try:
|
||||
n.arm()
|
||||
assert n.safety.aol_get_permission_mask() == 1
|
||||
a, d, b = n.packer.make_can_msg('SteeringPinion_Data', 0,
|
||||
{'StePinComp_An_Est': 100., 'StePinCompAnEst_D_Qf': 0, 'StePinAn_No_Cnt': (n.tick + 1) % 16})
|
||||
assert not n.safety.safety_rx_hook(libsafety_py.make_CANPacket(a, b, d))
|
||||
assert n.safety.aol_get_permission_mask() == 0
|
||||
assert n.safety.get_curvature_meas_max() == 0
|
||||
finally:
|
||||
n.tearDown()
|
||||
|
||||
|
||||
@pytest.mark.parametrize('failure', ['missing', 'wrong_bus', 'short', 'counter'])
|
||||
def test_pinion_integrity_is_required_not_substituted_by_healthy_yaw(failure):
|
||||
from opendbc.safety.tests.test_ford_aol import TestFordAolDriverIntent
|
||||
n = TestFordAolDriverIntent()
|
||||
n.setUp()
|
||||
n.reset(66)
|
||||
n.safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, 66 | ENABLE | 7 << SHIFT)
|
||||
n.safety.set_aol_test_heartbeat(True)
|
||||
original_rx = n.rx
|
||||
inject = True
|
||||
|
||||
def rx(name, values):
|
||||
if name == 'Yaw_Data_FD1':
|
||||
if inject:
|
||||
original_rx('SteeringPinion_Data', {'StePinComp_An_Est': 0., 'StePinCompAnEst_D_Qf': 3, 'StePinAn_No_Cnt': n.tick % 16})
|
||||
elif failure != 'missing':
|
||||
a, d, b = n.packer.make_can_msg('SteeringPinion_Data', 0,
|
||||
{'StePinComp_An_Est': 100., 'StePinCompAnEst_D_Qf': 3, 'StePinAn_No_Cnt': 2})
|
||||
n.safety.safety_rx_hook(libsafety_py.make_CANPacket(a, 1 if failure == 'wrong_bus' else b, d[:7] if failure == 'short' else d))
|
||||
original_rx(name, values)
|
||||
n.rx = rx
|
||||
try:
|
||||
n.arm()
|
||||
inject = False
|
||||
n.pump(120, mask=1)
|
||||
assert n.safety.aol_get_permission_mask() == 0
|
||||
finally:
|
||||
n.tearDown()
|
||||
|
||||
|
||||
@pytest.mark.parametrize('car', [c for c in GEOMETRY if 'MONDEO' not in c])
|
||||
@pytest.mark.parametrize('long', [False, True])
|
||||
def test_angle_selection_preserves_pinion_and_aol_exact_profile(car, long):
|
||||
from opendbc.bluepilot_lateral.hosts.starpilot import select as angle_select, qualified as angle_qualified
|
||||
from opendbc.car.ford.aol import base_safety_param, AOL_WORDS
|
||||
cp = params(car, long)
|
||||
assert select(cp, True)
|
||||
assert angle_select(cp, 1)
|
||||
assert configured(cp) and angle_qualified(cp)
|
||||
cp.safetyConfigs[-1].safetyParam |= base_safety_param(cp)
|
||||
cp.alternativeExperience = 32
|
||||
assert cp.safetyConfigs[-1].safetyParam in AOL_WORDS
|
||||
assert configured(cp) and angle_qualified(cp) and qualified(cp, marked_only=True)
|
||||
@@ -2,6 +2,7 @@
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/ford_aol.h"
|
||||
#include "opendbc/safety/modes/ford_pinion_measurement.h"
|
||||
#include "opendbc/bluepilot_lateral/safety/ford_bp.h"
|
||||
|
||||
// StarPilot's extended Ford curvature enforcement below is substantially adapted from
|
||||
@@ -16,6 +17,7 @@
|
||||
#define FORD_DesiredTorqBrk 0x213U // RX from ABS, for standstill state
|
||||
#define FORD_BrakeSysFeatures 0x415U // RX from ABS, for vehicle speed
|
||||
#define FORD_EngVehicleSpThrottle2 0x202U // RX from PCM, for second vehicle speed
|
||||
#define FORD_SteeringPinion_Data 0x7EU
|
||||
#define FORD_Yaw_Data_FD1 0x91U // RX from RCM, for yaw rate
|
||||
#define FORD_Steering_Data_FD1 0x083U // TX by OP, various driver switches and LKAS/CC buttons
|
||||
#define FORD_ACCDATA 0x186U // TX by OP, ACC controls
|
||||
@@ -40,6 +42,9 @@ static uint8_t ford_get_counter(const CANPacket_t *msg) {
|
||||
// Signal: VehRollYaw_No_Cnt
|
||||
cnt = msg->data[5];
|
||||
}
|
||||
if (msg->addr == FORD_SteeringPinion_Data) {
|
||||
cnt = (msg->data[5] >> 4) & 0xFU;
|
||||
}
|
||||
return cnt;
|
||||
}
|
||||
|
||||
@@ -86,6 +91,9 @@ static bool ford_get_quality_flag_valid(const CANPacket_t *msg) {
|
||||
if (msg->addr == FORD_Yaw_Data_FD1) {
|
||||
valid = ((msg->data[6] >> 4) & 0x3U) == 0x3U; // VehYawWActl_D_Qf
|
||||
}
|
||||
if (msg->addr == FORD_SteeringPinion_Data) {
|
||||
valid = ((msg->data[5] >> 2) & 0x3U) == 0x3U;
|
||||
}
|
||||
return valid;
|
||||
}
|
||||
|
||||
@@ -105,6 +113,17 @@ static const CurvatureSteeringLimits FORD_STEERING_LIMITS = {
|
||||
.max_steer_power = 0, // disabled, Ford has no steed power signal
|
||||
};
|
||||
|
||||
static CurvatureSteeringLimits ford_measurement_limits(CurvatureSteeringLimits limits) {
|
||||
return (CurvatureSteeringLimits){
|
||||
.max_curvature = limits.max_curvature,
|
||||
.curvature_to_can = limits.curvature_to_can,
|
||||
.frequency = limits.frequency,
|
||||
.max_curvature_error = ford_pinion_enabled ? SAFETY_MAX(limits.max_curvature_error, 150) : limits.max_curvature_error,
|
||||
.curvature_error_min_speed = limits.curvature_error_min_speed,
|
||||
.max_steer_power = limits.max_steer_power,
|
||||
};
|
||||
}
|
||||
|
||||
// Transit 0x3CA has 0.000005 rad/m per raw step and runs at 33 Hz. Keep its
|
||||
// command history separate from the inactive 0x3D3 heartbeat at 20 Hz.
|
||||
static const CurvatureSteeringLimits FORD_LKA_STEERING_LIMITS = {
|
||||
@@ -178,7 +197,7 @@ static void ford_rx_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
|
||||
// Update vehicle yaw rate
|
||||
if (msg_matches(msg, FORD_Yaw_Data_FD1, FORD_MAIN_BUS)) {
|
||||
if (msg_matches(msg, FORD_Yaw_Data_FD1, FORD_MAIN_BUS) && !ford_pinion_enabled) {
|
||||
// FIXME: safety can receive yaw before new vehicle speed, it should recompute meas on either received
|
||||
// Signal: VehYaw_W_Actl
|
||||
// TODO: we should use the speed which results in the closest angle measurement to the desired angle
|
||||
@@ -193,6 +212,14 @@ static void ford_rx_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
}
|
||||
|
||||
if (ford_pinion_enabled && msg_matches(msg, FORD_SteeringPinion_Data, FORD_MAIN_BUS)) {
|
||||
const int raw = ((msg->data[2] & 0x7FU) << 8) | msg->data[3];
|
||||
const float radians = (((float)raw * 0.1F) - 1600.0F) * 0.017453292519943295F;
|
||||
const float speed = SAFETY_MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1F);
|
||||
const float measured = radians * get_curvature_factor(speed, *ford_pinion_params) / ford_pinion_params->steer_ratio;
|
||||
update_sample(&curvature_state.meas, ROUND(measured * FORD_STEERING_LIMITS.curvature_to_can));
|
||||
}
|
||||
|
||||
// Update gas pedal
|
||||
if (msg_matches(msg, FORD_EngVehicleSpThrottle, FORD_MAIN_BUS)) {
|
||||
// Pedal position: (0.1 * val) in percent
|
||||
@@ -383,7 +410,7 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
|
||||
} else {
|
||||
violation |= raw_curvature_rate != FORD_INACTIVE_CURVATURE_RATE;
|
||||
}
|
||||
violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, FORD_STEERING_LIMITS);
|
||||
violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, ford_measurement_limits(FORD_STEERING_LIMITS));
|
||||
violation |= ford_lka_steering && steer_control_enabled;
|
||||
|
||||
if (violation) {
|
||||
@@ -443,7 +470,7 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
|
||||
violation |= ((desired_curvature * desired_path_angle) <= 0) || (combined_accel > 2.5F);
|
||||
violation |= SAFETY_ABS(desired_path_angle - ford_mach_e_path_angle_last) > 110;
|
||||
}
|
||||
violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, FORD_MACH_E_STEERING_LIMITS);
|
||||
violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, ford_measurement_limits(FORD_MACH_E_STEERING_LIMITS));
|
||||
} else if (ford_generic_canfd_extended) {
|
||||
// One generic CAN FD owner; Mach-E's wider error/path assist stays separate.
|
||||
static const struct lookup_t source_rate = {{5.0F, 16.0F, 25.0F}, {0.0025F, 0.0014F, 0.00018F}};
|
||||
@@ -460,10 +487,10 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
|
||||
} else {
|
||||
violation |= (desired_curvature != 0) || (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE);
|
||||
}
|
||||
violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, FORD_STEERING_LIMITS);
|
||||
violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, ford_measurement_limits(FORD_STEERING_LIMITS));
|
||||
} else {
|
||||
violation |= (raw_curvature_rate != FORD_CANFD_INACTIVE_CURVATURE_RATE) || (raw_path_angle != FORD_INACTIVE_PATH_ANGLE);
|
||||
violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, FORD_STEERING_LIMITS);
|
||||
violation |= steer_curvature_cmd_checks(desired_curvature, 0, steer_control_enabled, ford_measurement_limits(FORD_STEERING_LIMITS));
|
||||
}
|
||||
tx &= !violation;
|
||||
if (ford_generic_canfd_extended && !tx) {
|
||||
@@ -505,8 +532,15 @@ static void ford_aol_bind_tx(safety_config *ret, CanMsg workspace[7]) {
|
||||
}
|
||||
}
|
||||
|
||||
static RxCheck ford_rx_workspace[11];
|
||||
|
||||
static safety_config ford_init(uint16_t configured_param) {
|
||||
const uint16_t bp_param = configured_param & ~256U;
|
||||
const bool pinion_valid = ford_pinion_word_valid(configured_param);
|
||||
const uint16_t profile_param = ford_pinion_base_word(configured_param);
|
||||
const uint16_t geometry = (configured_param >> 10U) & 15U;
|
||||
ford_pinion_enabled = pinion_valid && ((configured_param & FORD_PINION_ENABLE) != 0U);
|
||||
ford_pinion_params = ford_pinion_enabled ? &ford_pinion_geometry[geometry] : &ford_pinion_geometry[0];
|
||||
const uint16_t bp_param = profile_param & ~256U;
|
||||
const bool joint_angle = ((unsigned int)alternative_experience == 32U) &&
|
||||
((bp_param == 146U) || (bp_param == 147U) || (bp_param == 160U) || (bp_param == 161U) || (bp_param == 194U) || (bp_param == 195U));
|
||||
const uint16_t param = joint_angle ? (bp_param & ~128U) : bp_param;
|
||||
@@ -672,7 +706,6 @@ static safety_config ford_init(uint16_t configured_param) {
|
||||
{.msg = {{0x176, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, {0}, {0}}},
|
||||
{.msg = {{FORD_Lane_Assist_Data3, 0, 8, 30U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, {0}, {0}}},
|
||||
};
|
||||
static RxCheck ford_aol_rx_workspace[10];
|
||||
_Static_assert(sizeof(ford_aol_default_rx) == (9U * sizeof(RxCheck)), "Ford default RX count");
|
||||
_Static_assert(sizeof(ford_aol_edge_rx) == (9U * sizeof(RxCheck)), "Ford Edge RX count");
|
||||
_Static_assert(sizeof(ford_aol_mondeo_rx) == (10U * sizeof(RxCheck)), "Ford Mondeo RX count");
|
||||
@@ -692,16 +725,16 @@ static safety_config ford_init(uint16_t configured_param) {
|
||||
}
|
||||
for (int i = 0; i < 10; i++) {
|
||||
if (i < selected_len) {
|
||||
ford_aol_rx_workspace[i] = selected_rx[i];
|
||||
const unsigned int addr = ford_aol_rx_workspace[i].msg[0].addr;
|
||||
ford_rx_workspace[i] = selected_rx[i];
|
||||
const unsigned int addr = ford_rx_workspace[i].msg[0].addr;
|
||||
if (manual && ((addr == 0x176U) || (addr == 0x230U) || (addr == 0x5AU))) {
|
||||
ford_aol_rx_workspace[i].msg[0].addr = 0x3C3U;
|
||||
ford_rx_workspace[i].msg[0].addr = 0x3C3U;
|
||||
}
|
||||
} else {
|
||||
ford_aol_rx_workspace[i] = (RxCheck){0};
|
||||
ford_rx_workspace[i] = (RxCheck){0};
|
||||
}
|
||||
}
|
||||
ret.rx_checks = ford_aol_rx_workspace;
|
||||
ret.rx_checks = ford_rx_workspace;
|
||||
ret.rx_checks_len = selected_len;
|
||||
ret.disable_forwarding = false;
|
||||
static CanMsg ford_aol_tx_msgs[7];
|
||||
@@ -711,6 +744,28 @@ static safety_config ford_init(uint16_t configured_param) {
|
||||
_Static_assert(sizeof(FORD_CANFD_LONG_TX_MSGS) <= sizeof(ford_aol_tx_msgs), "Ford CAN FD long TX capacity");
|
||||
ford_aol_bind_tx(&ret, ford_aol_tx_msgs);
|
||||
}
|
||||
if (ford_pinion_enabled) {
|
||||
// Append after profile-specific RX selection so AOL retains every existing check.
|
||||
if (ret.rx_checks_len <= 10) {
|
||||
const int count = ret.rx_checks_len;
|
||||
for (int i = 0; i < count; i++) {
|
||||
ford_rx_workspace[i] = ret.rx_checks[i];
|
||||
}
|
||||
ford_rx_workspace[count] = (RxCheck){.msg = {{FORD_SteeringPinion_Data, 0, 8, 100U,
|
||||
.max_counter = 15U, .ignore_checksum = true}, {0}, {0}}};
|
||||
ret.rx_checks = ford_rx_workspace;
|
||||
ret.rx_checks_len = count + 1;
|
||||
} else {
|
||||
ret.tx_msgs = NULL;
|
||||
ret.tx_msgs_len = 0;
|
||||
ford_aol_reset();
|
||||
}
|
||||
}
|
||||
if (!pinion_valid) {
|
||||
ret.tx_msgs = NULL;
|
||||
ret.tx_msgs_len = 0;
|
||||
ford_aol_reset();
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
#pragma once
|
||||
#include "opendbc/safety/modes/ford_pinion.h"
|
||||
|
||||
static bool ford_aol_manual = false;
|
||||
static bool ford_aol_enabled = false;
|
||||
@@ -202,7 +203,7 @@ static void ford_aol_full_engagement(bool cruise_rising) {
|
||||
|
||||
static bool ford_aol_param_valid(uint16_t param) {
|
||||
bool result = false;
|
||||
switch (param & ~256U) {
|
||||
switch (ford_pinion_base_word(param) & ~256U) {
|
||||
case 8U: case 9U: case 10U: case 12U: case 13U: case 18U: case 32U: case 33U: case 66U:
|
||||
result = true;
|
||||
break;
|
||||
@@ -220,7 +221,7 @@ static void ford_aol_configure(uint16_t param) {
|
||||
ford_aol_reset();
|
||||
ford_aol_enabled = ford_aol_param_valid(param) && ((unsigned int)alternative_experience == 32U);
|
||||
ford_aol_manual = (param & 256U) != 0U;
|
||||
ford_aol_param = param & ~(256U | 128U);
|
||||
ford_aol_param = ford_pinion_base_word(param) & ~(256U | 128U);
|
||||
if (ford_aol_enabled) {
|
||||
static const AolSafetyPolicy policy = {
|
||||
.reset = ford_aol_reset,
|
||||
|
||||
@@ -0,0 +1,31 @@
|
||||
#pragma once
|
||||
// Pinion measurement selection adapted from BluePilot PR #145 (fe5b0195).
|
||||
// See CREDITS.md and THIRD_PARTY_NOTICES.md. No live mode switching.
|
||||
#define FORD_PINION_ENABLE 0x200U
|
||||
#define FORD_PINION_MASK 0x3E00U
|
||||
|
||||
static bool ford_pinion_word_valid(uint16_t word) {
|
||||
bool valid = true;
|
||||
if ((word & FORD_PINION_MASK) != 0U) {
|
||||
const uint16_t index = (word >> 10U) & 15U;
|
||||
const uint16_t base = word & ~(FORD_PINION_MASK | 256U);
|
||||
valid = (word & FORD_PINION_ENABLE) != 0U;
|
||||
if ((index == 1U) || (index == 2U) || (index == 5U) || (index == 6U) || (index == 9U)) {
|
||||
valid &= (base == 32U) || (base == 33U) || (base == 128U) || (base == 129U) || (base == 160U) || (base == 161U);
|
||||
} else if ((index == 3U) || (index == 4U) || (index == 7U) || (index == 8U) || (index == 12U)) {
|
||||
valid &= (base == 66U) || (base == 67U) || (base == 130U) || (base == 131U) || (base == 194U) || (base == 195U);
|
||||
} else if (index == 11U) {
|
||||
valid &= (base == 18U) || (base == 19U) || (base == 130U) || (base == 131U) || (base == 146U) || (base == 147U);
|
||||
} else if (index == 10U) {
|
||||
valid &= (base == 10U) || (base == 11U);
|
||||
} else {
|
||||
valid = false;
|
||||
}
|
||||
}
|
||||
return valid;
|
||||
}
|
||||
|
||||
static uint16_t ford_pinion_base_word(uint16_t word) {
|
||||
return ford_pinion_word_valid(word) ? (word & ~FORD_PINION_MASK) : 0xFFFFU;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,21 @@
|
||||
#pragma once
|
||||
#define FORD_PINION_GEOMETRY_COUNT 12U
|
||||
static const AngleSteeringParams ford_pinion_geometry[FORD_PINION_GEOMETRY_COUNT + 1U] = {
|
||||
{.slip_factor = 0.0f, .steer_ratio = 1.0f, .wheelbase = 1.0f}, // 0: invalid
|
||||
{.slip_factor = -0.00062819555f, .steer_ratio = 17.7f, .wheelbase = 2.670f}, // 1: FORD_BRONCO_SPORT_MK1
|
||||
{.slip_factor = -0.00061892325f, .steer_ratio = 16.7f, .wheelbase = 2.710f}, // 2: FORD_ESCAPE_MK4
|
||||
{.slip_factor = -0.00061892325f, .steer_ratio = 16.7f, .wheelbase = 2.710f}, // 3: FORD_ESCAPE_MK4_5
|
||||
{.slip_factor = -0.00045454798f, .steer_ratio = 17.0f, .wheelbase = 3.690f}, // 4: FORD_EXPEDITION_MK4
|
||||
{.slip_factor = -0.00055447339f, .steer_ratio = 16.8f, .wheelbase = 3.025f}, // 5: FORD_EXPLORER_MK6
|
||||
{.slip_factor = -0.00062121569f, .steer_ratio = 15.0f, .wheelbase = 2.700f}, // 6: FORD_FOCUS_MK4
|
||||
{.slip_factor = -0.00045331952f, .steer_ratio = 16.9f, .wheelbase = 3.700f}, // 7: FORD_F_150_LIGHTNING_MK1
|
||||
{.slip_factor = -0.00042037149f, .steer_ratio = 17.0f, .wheelbase = 3.990f}, // 8: FORD_F_150_MK14
|
||||
{.slip_factor = -0.00054528036f, .steer_ratio = 17.0f, .wheelbase = 3.076f}, // 9: FORD_MAVERICK_MK1
|
||||
{.slip_factor = -0.00058852001f, .steer_ratio = 14.8f, .wheelbase = 2.850f}, // 10: FORD_MONDEO_MK5
|
||||
{.slip_factor = -0.00056209187f, .steer_ratio = 17.0f, .wheelbase = 2.984f}, // 11: FORD_MUSTANG_MACH_E_MK1
|
||||
{.slip_factor = -0.00051293030f, .steer_ratio = 17.0f, .wheelbase = 3.270f}, // 12: FORD_RANGER_MK2
|
||||
};
|
||||
|
||||
|
||||
static bool ford_pinion_enabled = false;
|
||||
static const AngleSteeringParams *ford_pinion_params = &ford_pinion_geometry[0];
|
||||
@@ -456,3 +456,11 @@ bool safety_test_hyundai_gv60_catalog_rejection(bool alternate) {
|
||||
#endif
|
||||
return rejected;
|
||||
}
|
||||
|
||||
// Pinion protocol introspection for exact host/native geometry and namespace tests.
|
||||
uint16_t get_ford_pinion_base(uint16_t word) { return ford_pinion_base_word(word); }
|
||||
bool get_ford_pinion_enabled(void) { return ford_pinion_enabled; }
|
||||
float get_ford_pinion_slip(int index) { return ford_pinion_geometry[index].slip_factor; }
|
||||
float get_ford_pinion_ratio(int index) { return ford_pinion_geometry[index].steer_ratio; }
|
||||
float get_ford_pinion_wheelbase(int index) { return ford_pinion_geometry[index].wheelbase; }
|
||||
int get_ford_pinion_rx_count(void) { return current_safety_config.rx_checks_len; }
|
||||
|
||||
@@ -42,6 +42,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"HondaBoschARadar", {PERSISTENT, BOOL, "1"}},
|
||||
{"HondaGasFactorParams", {PERSISTENT, FLOAT, "1.0"}},
|
||||
{"HondaWindFactorParams", {PERSISTENT, FLOAT, "1.0"}},
|
||||
{"FordPrefSteerAngleCurvature", {PERSISTENT, BOOL, "0"}},
|
||||
{"FordLateralMode", {PERSISTENT, INT, "0"}},
|
||||
{"FordHandsFreeCluster", {PERSISTENT, BOOL, "0"}},
|
||||
{"FordHumanTurnDetection", {PERSISTENT, BOOL, "1"}},
|
||||
|
||||
@@ -252,6 +252,18 @@ int main() {
|
||||
for (uint32_t word = 0U; word <= 65535U; ++word) {
|
||||
bool expected = false;
|
||||
for (const auto exact : ford_words) expected |= (word == exact) || (word == (exact | 256U));
|
||||
// Exact opt-in geometry families, independently enumerated from the native decoder.
|
||||
for (uint16_t geometry = 1U; geometry <= 12U; geometry++) {
|
||||
const bool classic = (geometry == 1U) || (geometry == 2U) || (geometry == 5U) || (geometry == 6U) || (geometry == 9U);
|
||||
const uint16_t curvature = classic ? 32U : ((geometry == 11U) ? 18U : ((geometry == 10U) ? 10U : 66U));
|
||||
for (uint16_t longitudinal = 0U; longitudinal <= 1U; longitudinal++) {
|
||||
for (uint16_t angle = 0U; angle <= 1U; angle++) {
|
||||
if ((geometry == 10U) && angle) continue;
|
||||
const uint16_t exact = 512U | (geometry << 10U) | curvature | longitudinal | (angle ? 128U : 0U);
|
||||
expected |= (word == exact) || (word == (exact | 256U));
|
||||
}
|
||||
}
|
||||
}
|
||||
auto ford = status();
|
||||
ford.safety_mode = 6U;
|
||||
ford.safety_param = static_cast<uint16_t>(word);
|
||||
|
||||
@@ -1,10 +1,11 @@
|
||||
#pragma once
|
||||
|
||||
#include "openpilot/selfdrive/pandad/aol_protocol.h"
|
||||
#include "opendbc/safety/modes/ford_pinion.h"
|
||||
|
||||
static inline bool ford_aol_param(uint16_t param) {
|
||||
bool result = false;
|
||||
switch (param & ~256U) {
|
||||
switch (ford_pinion_base_word(param) & ~256U) {
|
||||
case 8U: case 9U: case 10U: case 11U: case 12U: case 13U:
|
||||
case 18U: case 19U: case 32U: case 33U: case 66U: case 67U:
|
||||
case 146U: case 147U: case 160U: case 161U: case 194U: case 195U:
|
||||
|
||||
@@ -10,13 +10,17 @@ from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.starpilot.controllers.toyota_cruise import ToyotaCruisePreference, capability as toyota_cruise_capability
|
||||
|
||||
from opendbc.car.ford.values import CAR, FordFlags
|
||||
from opendbc.car.ford.pinion import configured as pinion_configured
|
||||
|
||||
|
||||
class ManualTurnInputs:
|
||||
def __init__(self, params, *, track_assist_permission: bool = False):
|
||||
def __init__(self, params, *, track_assist_permission: bool = False, pinion: bool = False):
|
||||
self.params = params
|
||||
self.pinion = pinion
|
||||
self.vehicle_parameters = None
|
||||
self.track_assist_permission = track_assist_permission
|
||||
self.sm = messaging.SubMaster(["modelV2", "lateralDelay"] + (["pandaStates", "aolSafetyWire"] if track_assist_permission else []))
|
||||
services = ["modelV2", "lateralDelay"] + (["pandaStates", "aolSafetyWire"] if track_assist_permission else [])
|
||||
self.sm = messaging.SubMaster(services + (["vehicleParameters"] if pinion else []))
|
||||
self.enabled = bool(self.params.get("FordHumanTurnDetection", return_default=True))
|
||||
self.frames = 0
|
||||
self.blend_settings = self._read_blend_settings()
|
||||
@@ -39,12 +43,16 @@ class ManualTurnInputs:
|
||||
def apply_blend_settings(self, controller):
|
||||
for name in ("mache_lateral", "classic_lateral", "new_port_curvature"):
|
||||
owner = getattr(controller, name, None)
|
||||
if owner is not None and self.pinion:
|
||||
owner.measurement.update_parameters(self.vehicle_parameters)
|
||||
if owner is not None and self._applied_blends.get(owner) != self.blend_settings:
|
||||
owner.set_blend_settings(*self.blend_settings)
|
||||
self._applied_blends[owner] = self.blend_settings
|
||||
|
||||
def update(self):
|
||||
self.sm.update(0)
|
||||
if self.pinion and self.sm.valid["vehicleParameters"] and self.sm.alive["vehicleParameters"]:
|
||||
self.vehicle_parameters = self.sm["vehicleParameters"]
|
||||
if self.frames % 100 == 0:
|
||||
self.enabled = bool(self.params.get("FordHumanTurnDetection", return_default=True))
|
||||
self.blend_settings = self._read_blend_settings()
|
||||
@@ -249,20 +257,20 @@ def configure_controller(CI, params):
|
||||
if (controller is not None and cp.brand == "ford" and cp.carFingerprint == CAR.FORD_MUSTANG_MACH_E_MK1 and
|
||||
not cp.flags & FordFlags.LKA_STEERING and not cp.passive and not cp.dashcamOnly and not cp.notCar and
|
||||
getattr(controller, "manual_turn", None) is not None):
|
||||
controller.manual_turn_inputs = ManualTurnInputs(params, track_assist_permission=bool(cp.flags & FordFlags.CANFD))
|
||||
controller.manual_turn_inputs = ManualTurnInputs(params, track_assist_permission=bool(cp.flags & FordFlags.CANFD), pinion=pinion_configured(cp))
|
||||
|
||||
from opendbc.bluepilot_lateral.hosts.starpilot import qualified as angle_qualified
|
||||
if controller is not None and angle_qualified(cp) and getattr(controller, "bp_lat", None) is not None:
|
||||
controller.bp_lat.host.inputs = ManualTurnInputs(params, track_assist_permission=True)
|
||||
controller.bp_lat.host.inputs = ManualTurnInputs(params, track_assist_permission=True, pinion=pinion_configured(cp))
|
||||
|
||||
from opendbc.car.ford.classic_lateral import qualified as classic_qualified
|
||||
from opendbc.car.ford.generic_canfd_lateral import qualified as generic_canfd_qualified
|
||||
if controller is not None and (classic_qualified(cp) or generic_canfd_qualified(cp)) and getattr(controller, "classic_lateral", None) is not None:
|
||||
controller.manual_turn_inputs = ManualTurnInputs(params)
|
||||
controller.manual_turn_inputs = ManualTurnInputs(params, pinion=pinion_configured(cp))
|
||||
|
||||
from opendbc.car.ford.new_port_curvature import qualified as new_port_curvature_qualified
|
||||
if controller is not None and new_port_curvature_qualified(cp) and getattr(controller, "new_port_curvature", None) is not None:
|
||||
controller.manual_turn_inputs = ManualTurnInputs(params)
|
||||
controller.manual_turn_inputs = ManualTurnInputs(params, pinion=pinion_configured(cp))
|
||||
|
||||
if controller is not None and cp.brand == "ford" and getattr(controller, "manual_turn_inputs", None) is not None:
|
||||
controller.manual_turn_inputs.apply_blend_settings(controller)
|
||||
|
||||
@@ -0,0 +1,61 @@
|
||||
"""Actual typed Params, Galaxy parked write, and startup configuration for pinion."""
|
||||
from pathlib import Path
|
||||
import tempfile
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
from opendbc.car import gen_empty_fingerprint
|
||||
from opendbc.car.ford.pinion import KEY, GEOMETRY, configured
|
||||
from opendbc.car.ford.values import CAR
|
||||
from opendbc.car.ford.tests.test_three_ports import params as ford_params
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.starpilot.galaxy.settings import AuthorityContext, SettingsGateway
|
||||
|
||||
|
||||
def test_real_galaxy_typed_parked_selection_and_stale_dependency():
|
||||
with tempfile.TemporaryDirectory() as directory:
|
||||
params = Params(directory)
|
||||
assert params.get_default_value(KEY) is False
|
||||
params.put_bool('SafeMode', False, block=True)
|
||||
params.put_bool('AlwaysOnLateral', False, block=True)
|
||||
cp = ford_params(CAR.FORD_EXPLORER_MK6)
|
||||
parked = True
|
||||
context = SimpleNamespace(sample=lambda: AuthorityContext(parked, cp, cp.as_reader().as_builder().to_bytes()))
|
||||
gateway = SettingsGateway(params, context)
|
||||
try:
|
||||
page = gateway.page('torque', 'pinion-test', b'generation')
|
||||
index, row = next((i, row) for i, row in enumerate(page['rows']) if row['label'] == 'Use Pinion Yaw Sensor')
|
||||
assert row['choices'] == ['Off', 'On'] and row['available']
|
||||
intent = gateway.preview(page['view'], index, 0, 'pinion-test', b'generation', value='On')
|
||||
assert gateway.confirm(intent['intent'], 'pinion-test', b'generation')
|
||||
assert params.get(KEY) is True
|
||||
assert Path(params.get_param_path(KEY)).read_bytes() == b'1'
|
||||
assert not configured(cp) # Saved preference cannot mutate the current driving configuration.
|
||||
parked = False
|
||||
page = gateway.page('torque', 'pinion-test', b'generation')
|
||||
row = next(row for row in page['rows'] if row['label'] == 'Use Pinion Yaw Sensor')
|
||||
assert not row['available']
|
||||
finally:
|
||||
gateway.close()
|
||||
|
||||
|
||||
@pytest.mark.parametrize('identity', list(GEOMETRY))
|
||||
def test_actual_factory_applies_before_controller_and_published_cp(identity):
|
||||
from openpilot.starpilot.tests.test_vehicle_preferences import TestVehicleStartupPreferences
|
||||
helper = TestVehicleStartupPreferences()
|
||||
helper.setUp()
|
||||
try:
|
||||
helper.raw('SafeMode', b'0')
|
||||
helper.raw('AlwaysOnLateral', b'0')
|
||||
helper.params.put_bool('AlphaLongitudinalEnabled', False, block=True)
|
||||
helper.params.put_bool('IsReleaseBranch', False, block=True)
|
||||
fp = gen_empty_fingerprint()
|
||||
fp[0][0x5A] = 8
|
||||
fp[2].update({0x3D6: 8, 0x186: 8})
|
||||
host, constructed, published = helper.start(identity, key=KEY, requested=True, observed=fp,
|
||||
capture=lambda ci: configured(ci.CP))
|
||||
assert constructed == [True]
|
||||
assert configured(published)
|
||||
assert host.CI.CP.to_dict() == published.to_dict()
|
||||
finally:
|
||||
helper.doCleanups()
|
||||
@@ -53,6 +53,7 @@ from openpilot.starpilot.ui.wheel_feature import WheelFeature, PREFIX as WHEEL_P
|
||||
from openpilot.starpilot.longitudinal.output_max import KEY as OUTPUT_MAX_KEY
|
||||
from openpilot.starpilot.ui.tesla_screen_feature import TeslaScreenFeature, KEYS as TESLA_SCREEN_KEYS
|
||||
from openpilot.starpilot.ui.ford_lateral_feature import FordLateralFeature, FordClusterFeature
|
||||
from openpilot.starpilot.ui.ford_pinion_feature import FordPinionFeature
|
||||
from openpilot.starpilot.ui.preap_radar_feature import PreAPRadarFeature, KEYS as PREAP_RADAR_KEYS, capability as preap_radar_capability
|
||||
from openpilot.starpilot.ui.gm_tune_feature import GmTuneFeature, GmTruckFeature, GmEvPresetFeature
|
||||
from openpilot.starpilot.car.gm.tune_preferences import KEY as GM_TUNE_KEY
|
||||
@@ -992,6 +993,7 @@ class FeatureSettingsOwner:
|
||||
elif page == FeaturePage.TORQUE:
|
||||
title = "Steering and Torque"
|
||||
rows.extend(FordLateralFeature(self).rows(parked))
|
||||
rows.extend(FordPinionFeature(self).rows(parked))
|
||||
rows.extend(FordClusterFeature(self).rows(parked))
|
||||
controller_row = self.controller.row()
|
||||
if controller_row is not None:
|
||||
@@ -1309,6 +1311,8 @@ class FeatureSettingsOwner:
|
||||
return self._apply_lateral_pause(request)
|
||||
if key == "FordHandsFreeCluster":
|
||||
return FordClusterFeature(self).apply(request)
|
||||
if key == "FordPrefSteerAngleCurvature":
|
||||
return FordPinionFeature(self).apply(request)
|
||||
if key == "FordLateralMode":
|
||||
return FordLateralFeature(self).apply(request)
|
||||
if key == "TurnAssist":
|
||||
|
||||
@@ -0,0 +1,74 @@
|
||||
"""Parked, init-only pinion measurement preference shared by native UI and Galaxy."""
|
||||
from opendbc.car.ford.aol import qualified as aol_qualified
|
||||
from opendbc.car.ford.pinion import KEY
|
||||
from openpilot.starpilot.saved_source import read_saved
|
||||
from openpilot.starpilot.saved_document import commit_exact
|
||||
from openpilot.starpilot.ui.feature_settings_state import FeatureRow
|
||||
|
||||
CHOICES = {0: "Off", 1: "On"}
|
||||
|
||||
|
||||
def read_choice(params):
|
||||
raw, readable = read_saved(params, KEY, 8)
|
||||
valid = readable and raw in (None, b"0", b"1")
|
||||
return int(valid and raw == b"1"), raw, readable, valid
|
||||
|
||||
|
||||
class FordPinionFeature:
|
||||
def __init__(self, owner):
|
||||
self.owner = owner
|
||||
|
||||
def capability(self):
|
||||
cp = self.owner.vehicle_params()
|
||||
try:
|
||||
if cp is None:
|
||||
return None
|
||||
from opendbc.car.ford.pinion import GEOMETRY
|
||||
if cp.carFingerprint not in GEOMETRY or not aol_qualified(cp):
|
||||
return None
|
||||
return (str(cp.carFingerprint), int(cp.flags), int(cp.alternativeExperience), bool(cp.openpilotLongitudinalControl),
|
||||
bool(cp.pcmCruise), str(cp.steerControlType), bool(cp.passive), bool(cp.dashcamOnly), bool(cp.notCar),
|
||||
tuple((str(c.safetyModel), int(c.safetyParam)) for c in cp.safetyConfigs))
|
||||
except (AttributeError, TypeError, ValueError, OverflowError):
|
||||
return None
|
||||
|
||||
def dependencies(self):
|
||||
values = []
|
||||
for key in ("SafeMode", "AlwaysOnLateral"):
|
||||
raw, readable = read_saved(self.owner.params, key, 8)
|
||||
if not readable or raw not in ((None, b"0") if key == "SafeMode" else (None, b"0", b"1")):
|
||||
return None
|
||||
values.append((key, raw))
|
||||
return tuple(values)
|
||||
|
||||
def rows(self, parked):
|
||||
capability = self.capability()
|
||||
if capability is None:
|
||||
return ()
|
||||
value, raw, readable, valid = read_choice(self.owner.params)
|
||||
dependencies = self.dependencies()
|
||||
return (FeatureRow(KEY, "Use Pinion Yaw Sensor", CHOICES[value] if valid else "Invalid saved choice", raw,
|
||||
choices=tuple(CHOICES.values()), available=parked and self.owner.authority("parked_preferences") and
|
||||
readable and dependencies is not None, capability=capability,
|
||||
dependencies=dependencies or (), repair_value="" if valid else CHOICES[0], default_value=CHOICES[0],
|
||||
reason="Off by default. Uses steering-pinion curvature instead of RCM yaw in control and Panda safety. "
|
||||
+ "Applies next vehicle startup; Safe Mode must be off."),)
|
||||
|
||||
def apply(self, request):
|
||||
value = next((code for code, label in CHOICES.items() if label == request.value), None)
|
||||
if (request.key != KEY or value is None or request.related_source is not None or request.display_unit or request.direction):
|
||||
return False
|
||||
|
||||
def authorized():
|
||||
capability = self.capability()
|
||||
_, raw, readable, valid = read_choice(self.owner.params)
|
||||
dependencies = self.dependencies()
|
||||
return (self.owner.authority("parked_preferences") and bool(request.vehicle_fingerprint) and
|
||||
self.owner.vehicle_fingerprint() == request.vehicle_fingerprint and capability is not None and
|
||||
capability == request.capability and dependencies is not None and dependencies == request.dependencies and
|
||||
readable and raw == request.expected and (valid or value == 0))
|
||||
|
||||
result = commit_exact(self.owner.params, key=KEY, max_bytes=8, raw=str(value).encode(), expected=request.expected,
|
||||
authorized=authorized, temp_prefix=".ford-pinion-")
|
||||
return result.committed and result.verified
|
||||
|
||||
@@ -42,6 +42,7 @@ class VehicleStartupPreferences:
|
||||
toyota_filter: bool = True
|
||||
gm_longitudinal_tune: int = 0
|
||||
ford_lateral_mode: int = 0
|
||||
ford_pinion: bool = field(default=False, kw_only=True)
|
||||
toyota_lock_doors: bool = False
|
||||
toyota_unlock_doors: bool = False
|
||||
toyota_radar_mode: int = 0
|
||||
@@ -112,7 +113,9 @@ class VehicleStartupPreferences:
|
||||
subaru_sng=bool(enabled and readable and safe in (None, b"0") and read_saved(params, "SubaruSNG", 8) == (b"1", True)),
|
||||
subaru_software_cruise=bool(enabled and readable and safe in (None, b"0") and
|
||||
read_saved(params, "SubaruRedneckCruise", 8) == (b"1", True)), toyota_radar_mode=toyota_radar_mode(params, enabled=enabled),
|
||||
ford_lateral_mode=ford_mode(params, enabled=enabled), tesla_screen=screen,
|
||||
ford_lateral_mode=ford_mode(params, enabled=enabled),
|
||||
ford_pinion=bool(enabled and read_saved(params, "SafeMode", 8) in ((None, True), (b"0", True)) and
|
||||
read_saved(params, "FordPrefSteerAngleCurvature", 8) == (b"1", True)), tesla_screen=screen,
|
||||
tesla_screen_brake=screen_brake, toyota_auto_hold=toyota, jeep_brake_hold=jeep_hold,
|
||||
gm_longitudinal_tune=selected_tune(params, enabled=enabled),
|
||||
toyota_lock_doors=bool(enabled and readable and safe in (None, b"0") and
|
||||
@@ -221,6 +224,8 @@ class VehicleStartupPreferences:
|
||||
apply_radar_mode(cp, self.toyota_radar_mode)
|
||||
from opendbc.bluepilot_lateral.hosts.starpilot import select as select_ford_lateral
|
||||
select_ford_lateral(cp, self.ford_lateral_mode)
|
||||
from opendbc.car.ford.pinion import select as select_pinion
|
||||
select_pinion(cp, self.ford_pinion)
|
||||
return cp
|
||||
|
||||
def finalize(self, cp) -> None:
|
||||
|
||||
+1
-1
@@ -1 +1 @@
|
||||
{"protected_inventory_sha256": "13911302060b8abd2039f8860f69bfe5e78a40a9b05d68eeb09b9d47254a4450", "schema_version": 1}
|
||||
{"protected_inventory_sha256": "e586cac372850cf9e03c7a677d3477ae1e3b685d47cd1767faf424579acb0631", "schema_version": 1}
|
||||
|
||||
Reference in New Issue
Block a user