Keep Ford hands-free display active through driver turns

This commit is contained in:
firestar5683
2026-10-23 13:10:24 -05:00
parent 657a9462ce
commit dcb7d5de82
8 changed files with 351 additions and 12 deletions
@@ -75,7 +75,7 @@ class CarController(CarControllerBase):
self.manual_turn_inputs = None
self.mache_lateral = MachELateralController(CP) if mache_qualified(CP) else None
self.mache_extended_announced = False
self.mache_ui_active = False
self.mache_ui_owned = False
self.hands_free_cluster_input = None
self.cluster_extended_announced = False
self.generic_canfd = generic_canfd_qualified(CP)
@@ -104,7 +104,7 @@ class CarController(CarControllerBase):
if self.hands_free_cluster_input is not None:
self.hands_free_cluster_input.update()
if not CC.latActive:
self.mache_ui_active = False
self.mache_ui_owned = False
if (self.mache_lateral is not None or self.classic_lateral is not None or self.new_port_curvature is not None) and self.manual_turn_inputs is not None:
self.manual_turn_inputs.update()
self.manual_turn_inputs.apply_blend_settings(self)
@@ -163,7 +163,7 @@ class CarController(CarControllerBase):
demanded = self.mache_lateral.update(CC, CS, actuators) if self.mache_extended_announced else FordLateralResult()
lateral = bounded_command(self.mache_lateral, demanded, previous, CS.out.vEgoRaw, previous_path_angle)
self.mache_lateral_demand = demanded
self.mache_ui_active = bool(lateral.active)
self.mache_ui_owned |= bool(lateral.active)
self.apply_curvature_last = lateral.curvature
counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10
can_sends.append(mache_can.create_lat_ctl2_msg(
@@ -335,10 +335,10 @@ class CarController(CarControllerBase):
self.cluster_extended_announced = True
### ui ###
# AOL safety forwards stock UI until an active lateral replacement owns it.
# Avoid competing cluster messages while stock remains the display owner.
# Acquire the display after the first active replacement, then retain it while
# AOL stays active, including a manual-turn steering handoff (as on Dom).
aol_mache_ui = self.mache_lateral is not None and qualified_aol(self.CP, marked_only=True)
ui_enabled = CC.latActive and (not aol_mache_ui or self.mache_ui_active)
ui_enabled = CC.latActive and (not aol_mache_ui or self.mache_ui_owned)
if not aol_mache_ui or ui_enabled:
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != ui_enabled) or (self.steer_alert_last != steer_alert)
# send lkas ui msg at 1Hz or if ui state changes
@@ -0,0 +1,139 @@
"""Recorded Mach-E turn inputs retain Dom display state across steering handoff."""
from pathlib import Path
from types import SimpleNamespace
import pytest
from opendbc.can import CANParser
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.values import CAR, DBC
from opendbc.safety.tests.libsafety import libsafety_py
from opendbc.safety.tests import test_ford_aol
from openpilot.common.params import Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.starpilot.car.ford.cluster_preferences import FordClusterPreference, KEY
class RecordedPreview:
def __init__(self):
self.model = SimpleNamespace(orientationRate=SimpleNamespace(z=[0.] * 33),
meta=SimpleNamespace(laneChangeState=0, laneChangeDirection=0))
def update(self):
pass
def apply_blend_settings(self, controller):
controller.mache_lateral.set_blend_settings(.4, .4, .85)
def lateral_snapshot(self, speed):
return self.model, ModelConstants.T_IDXS, .2, True
def assist_permission(self):
return (), False
def run_turn(*, hands_free, angle, torque, desired, speed, yaw):
native = test_ford_aol.TestFordAolDriverIntent()
native.setUp()
native.reset(18)
native.arm()
with OpenpilotPrefix():
saved = Params()
Path(saved.get_param_path(KEY)).write_bytes(b"1" if hands_free else b"0")
fingerprint = gen_empty_fingerprint()
fingerprint[0].update({0x5A: 8, 0x3A6: 8, 0x3A7: 8})
fingerprint[2].update({0x3D6: 8, 0x186: 8})
cp = CarInterface.get_params(CAR.FORD_MUSTANG_MACH_E_MK1, fingerprint, [], False, False, False)
cp.alternativeExperience = 32
assert cp.safetyConfigs[-1].safetyParam == 18
assert cp.flags == 3
controller = CarController(DBC[cp.carFingerprint], cp)
controller.hands_free_cluster_input = FordClusterPreference(saved)
controller.manual_turn_inputs = RecordedPreview()
state = CarState(cp)
state.update(state.get_can_parsers(cp))
cs = structs.CarState(canValid=True, vEgo=speed, vEgoRaw=speed, yawRate=yaw,
gearShifter=structs.CarState.GearShifter.drive)
cs.cruiseState.available = True
command = structs.CarControl(latActive=False)
command.actuators.curvature = desired
state.acc_tja_status_stock_values['FcwVisblWarn_B_Rq'] = 1
state.acc_tja_status_stock_values['FcwAudioWarn_B_Rq'] = 1
parser = CANParser('ford_lincoln_base_pt', [('ACCDATA_3', 5)], 0)
steering = []
displays = []
def step():
native.pump(1, mask=int(command.latActive), speed=speed, curvature=yaw / speed)
state.out = cs.as_reader()
_, frames = controller.update(command.as_reader(), state, (1_000_000 + native.tick * 10_000) * 1000)
for address, data, bus in frames:
assert native.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data))
if address == 0x3D6:
steering.append(bytes(data))
if address == 0x18A:
parser.update([(native.tick * 10_000_000, [(address, data, bus)])])
displays.append(dict(parser.vl['ACCDATA_3']))
return frames
# Startup remains stock until an active replacement is actually accepted.
for _ in range(25):
assert not {address for address, _, _ in step()} & {0x18A, 0x3D8}
assert native.safety.safety_fwd_hook(2, 0x18A) == 0
native.arm()
command.latActive = True
for _ in range(60):
step()
assert displays and displays[-1]['Tja_D_Stat'] == (7 if hands_free else 2)
assert native.safety.aol_get_permission_mask() == 1
assert native.safety.safety_fwd_hook(2, 0x18A) == -1
# Both original onset tuples have driver input, a right signal and >12deg.
cs.steeringPressed = True
cs.steeringAngleDeg = angle
cs.steeringTorque = torque
cs.rightBlinker = True
count = len(displays)
for _ in range(40):
frames = step()
if any(address == 0x3D6 for address, _, _ in frames):
assert ((steering[-1][0] >> 4) & 7) == 0
assert native.safety.safety_fwd_hook(2, 0x3D6) == 0
assert native.safety.safety_fwd_hook(2, 0x18A) == -1
assert native.safety.safety_fwd_hook(2, 0x3D8) == -1
assert native.safety.aol_get_permission_mask() == 1
assert len(displays) > count
for display in displays[count:]:
assert display['Tja_D_Stat'] == (7 if hands_free else 2)
assert display['FcwVisblWarn_B_Rq'] == 1
assert display['FcwAudioWarn_B_Rq'] == 1
cs.steeringPressed = False
cs.steeringAngleDeg = 0.
cs.steeringTorque = 0.
cs.rightBlinker = False
for _ in range(50):
step()
assert ((steering[-1][0] >> 4) & 7) == 1
assert displays[-1]['Tja_D_Stat'] == (7 if hands_free else 2)
command.latActive = False
for _ in range(25):
assert not {address for address, _, _ in step()} & {0x18A, 0x3D8}
assert native.safety.safety_fwd_hook(2, 0x18A) == 0
assert native.safety.safety_fwd_hook(2, 0x3D8) == 0
assert native.safety.aol_get_permission_mask() == 0
return steering
@pytest.mark.parametrize('angle,torque,desired,speed,yaw', [
(-12.2, -1.6875, .000041086183, 2.55, -.006),
(-38.7, -2., -.000324735534, 2.563888788, -.033),
])
def test_recorded_turn_keeps_dom_display_and_hands_free_does_not_change_steering(angle, torque, desired, speed, yaw):
hands_free = run_turn(hands_free=True, angle=angle, torque=torque, desired=desired, speed=speed, yaw=yaw)
plain = run_turn(hands_free=False, angle=angle, torque=torque, desired=desired, speed=speed, yaw=yaw)
assert plain == hands_free
+4
View File
@@ -239,6 +239,10 @@ static bool ford_tx_hook(const CANPacket_t *msg) {
bool tx = true;
if ((msg->addr == FORD_ACCDATA_3) || (msg->addr == FORD_IPMA_Data)) {
tx = ford_aol_display_tx_allowed();
}
// Safety check for ACCDATA accel and brake requests
if (msg->addr == FORD_ACCDATA) {
// Signal: AccPrpl_A_Rq
+40 -5
View File
@@ -16,7 +16,10 @@ static uint16_t ford_aol_param = 0U;
static bool ford_aol_drive = false;
static uint8_t ford_aol_eps = 0U;
static bool ford_aol_lka_ready = false;
static bool ford_aol_owned = false;
// Keep steering and display ownership separate without adding state storage.
#define FORD_AOL_STEERING_OWNED 1U
#define FORD_AOL_DISPLAY_OWNED 2U
static uint8_t ford_aol_owned = 0U;
static uint32_t ford_aol_accepted_ts = 0U;
static void ford_aol_clear(void) {
@@ -28,7 +31,7 @@ static void ford_aol_clear(void) {
ford_aol_token_ts = 0U;
ford_aol_main_neutral = false;
ford_aol_button_neutral = false;
ford_aol_owned = false;
ford_aol_owned = 0U;
}
static void ford_aol_reset(void) {
@@ -59,6 +62,9 @@ static void ford_aol_arm(void) {
static void ford_aol_host_request(uint8_t mask) {
ford_aol_request_seen |= mask != 0U;
if (((ford_aol_param & 0xFFFEU) == 18U) && ((mask & 1U) == 0U)) {
ford_aol_owned &= FORD_AOL_STEERING_OWNED;
}
}
static uint8_t ford_aol_request_mask(void) {
@@ -109,6 +115,9 @@ static uint8_t ford_aol_permission_mask(void) {
!brake_pressed && !gas_pressed && !gas_pressed_prev) {
result |= 2U;
}
if ((base == 18U) && ((result & 1U) == 0U)) {
ford_aol_owned &= FORD_AOL_STEERING_OWNED;
}
return result;
}
@@ -165,6 +174,12 @@ static void ford_aol_rx(const CANPacket_t *msg) {
} else {
// Manual profiles consume only the dedicated reverse lamp source.
}
// A transient physical veto revokes only display ownership, even when a
// healthy packet follows in the same RX batch. AOL intent is retained.
if (((ford_aol_param & 0xFFFEU) == 18U) &&
(!ford_aol_main_available || !ford_aol_drive || (ford_aol_eps != 0U) || !ford_aol_lka_ready)) {
ford_aol_owned &= FORD_AOL_STEERING_OWNED;
}
}
}
@@ -222,16 +237,36 @@ static void ford_aol_tx(const CANPacket_t *msg, bool accepted) {
const bool active = (msg->addr == 0x3CAU) ? (((msg->data[0] >> 5U) == 2U) || ((msg->data[0] >> 5U) == 4U)) :
((msg->addr == 0x3D3U) ? (((msg->data[4] >> 2U) & 7U) != 0U) :
(((msg->data[0] >> 4U) & 7U) != 0U));
ford_aol_owned = accepted && active && (ford_aol_permission_mask() != 0U) && !relay_malfunction;
const bool mache = (ford_aol_param & 0xFFFEU) == 18U;
const bool retained_display = mache && ((ford_aol_owned & FORD_AOL_DISPLAY_OWNED) != 0U) &&
(safety_get_ts_elapsed(microsecond_timer_get(), ford_aol_accepted_ts) <= AOL_HOST_REQUEST_TIMEOUT_US);
const uint8_t permission = (accepted && (active || retained_display)) ? ford_aol_permission_mask() : 0U;
const bool steering_owned = accepted && active && (permission != 0U) && !relay_malfunction;
const bool display_owned = steering_owned || (accepted && retained_display && ((permission & 1U) != 0U) && !relay_malfunction);
ford_aol_owned = (steering_owned ? FORD_AOL_STEERING_OWNED : 0U) | (display_owned ? FORD_AOL_DISPLAY_OWNED : 0U);
ford_aol_accepted_ts = microsecond_timer_get();
}
}
static bool ford_aol_fwd(int bus, int addr) {
bool blocked = false;
if (ford_aol_enabled && (bus == 2) && ford_aol_owned && (ford_aol_permission_mask() != 0U) &&
if (ford_aol_enabled && (bus == 2) && (ford_aol_owned != 0U) && (ford_aol_permission_mask() != 0U) &&
(safety_get_ts_elapsed(microsecond_timer_get(), ford_aol_accepted_ts) <= AOL_HOST_REQUEST_TIMEOUT_US)) {
blocked = (addr == (int)ford_aol_replacement()) || (addr == 0x3D8) || (addr == 0x18A);
const bool display_permitted = ((ford_aol_param & 0xFFFEU) != 18U) || ((ford_aol_permission_mask() & 1U) != 0U);
blocked = (((ford_aol_owned & FORD_AOL_STEERING_OWNED) != 0U) && (addr == (int)ford_aol_replacement())) ||
(((ford_aol_owned & FORD_AOL_DISPLAY_OWNED) != 0U) && display_permitted && ((addr == 0x3D8) || (addr == 0x18A)));
}
return blocked;
}
// An unowned Mach-E display must remain stock, including a rejected first
// steering command. This cannot grant steering or longitudinal permission.
static bool ford_aol_display_tx_allowed(void) {
bool allowed = true;
if (ford_aol_enabled && ((ford_aol_param & 0xFFFEU) == 18U)) {
allowed = ((ford_aol_owned & FORD_AOL_DISPLAY_OWNED) != 0U) &&
((ford_aol_permission_mask() & 1U) != 0U) && !relay_malfunction &&
(safety_get_ts_elapsed(microsecond_timer_get(), ford_aol_accepted_ts) <= AOL_HOST_REQUEST_TIMEOUT_US);
}
return allowed;
}
@@ -0,0 +1,155 @@
"""Display continuity never grants steering or survives loss of native authority."""
import pytest
from opendbc.car import gen_empty_fingerprint
from opendbc.car.ford import mache_can, fordcan
from opendbc.car.ford.fordcan import CanBus
from opendbc.safety.tests.libsafety import libsafety_py
from opendbc.safety.tests import test_ford_aol
@pytest.fixture(params=(18, 19, 274, 275))
def owner(request):
native = test_ford_aol.TestFordAolDriverIntent()
native.setUp()
native.reset(request.param)
# Standard matured relay setup, with the same configured AE before and after.
native.safety.init_tests()
native.safety.set_alternative_experience(32)
native.safety.set_aol_test_heartbeat(True)
native.arm()
return native
def command(owner, *, active, curvature=0., offset_byte=False):
frame = mache_can.create_lat_ctl2_msg(owner.packer, CanBus(fingerprint=gen_empty_fingerprint()),
int(active), 0, 1, curvature, 0., 0)
data = bytearray(frame[1])
if offset_byte:
data[5] ^= 1
return libsafety_py.make_CANPacket(frame[0], frame[2], data)
def announce(owner):
frame = mache_can.create_lka_msg(owner.packer, CanBus(fingerprint=gen_empty_fingerprint()))
assert owner.safety.safety_tx_hook(libsafety_py.make_CANPacket(frame[0], frame[2], frame[1]))
def acquire(owner):
announce(owner)
assert owner.safety.safety_tx_hook(command(owner, active=True))
assert owner.safety.safety_fwd_hook(2, 0x3D6) == -1
assert owner.safety.safety_fwd_hook(2, 0x18A) == -1
assert owner.safety.aol_get_permission_mask() == 1
def stock_display(owner, *, relay=False):
for address in (0x18A, 0x3D8):
assert owner.safety.safety_fwd_hook(2, address) == (-1 if relay else 0)
assert not owner.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, 0, bytes(8)))
@pytest.mark.parametrize('first', ('none', 'inactive', 'rejected'))
def test_display_needs_first_accepted_active_command(owner, first):
announce(owner)
if first == 'inactive':
assert owner.safety.safety_tx_hook(command(owner, active=False))
elif first == 'rejected':
assert not owner.safety.safety_tx_hook(command(owner, active=True, offset_byte=True))
stock_display(owner)
def test_display_survives_fresh_inactive_packets_without_steering_ownership(owner):
acquire(owner)
for _ in range(30):
owner.pump(5, mask=1)
assert owner.safety.safety_tx_hook(command(owner, active=False))
assert owner.safety.safety_fwd_hook(2, 0x3D6) == 0
for address in (0x18A, 0x3D8):
assert owner.safety.safety_fwd_hook(2, address) == -1
assert owner.safety.safety_tx_hook(libsafety_py.make_CANPacket(address, 0, bytes(8)))
assert owner.safety.safety_fwd_hook(2, 0x321) == 0
assert owner.safety.aol_get_permission_mask() == 1
assert not owner.safety.get_controls_allowed()
@pytest.mark.parametrize('loss', ('rejected', 'expired', 'host_lat', 'heartbeat', 'eps', 'main', 'cancel', 'rx', 'relay', 'eps_temp', 'lka', 'gear'))
def test_display_drops_on_actual_authority_loss_and_does_not_reacquire_inactive(owner, loss):
acquire(owner)
assert owner.safety.safety_tx_hook(command(owner, active=False))
if loss == 'rejected':
assert not owner.safety.safety_tx_hook(command(owner, active=True, offset_byte=True))
elif loss == 'expired':
owner.pump(31, mask=1)
elif loss == 'host_lat':
owner.safety.aol_set_host_request(0)
elif loss == 'heartbeat':
owner.pump(2, mask=1, heartbeat=False)
elif loss == 'eps':
owner.rx('EPAS_INFO', {'SteeringColumnTorque': 0, 'EPAS_Failure': 2})
elif loss == 'eps_temp':
owner.rx('EPAS_INFO', {'SteeringColumnTorque': 0, 'EPAS_Failure': 1})
elif loss == 'lka':
owner.rx('Lane_Assist_Data3_FD1', {'LatCtlSte_D_Stat': 0, 'LaActAvail_D_Actl': 0, 'LaActDeny_B_Actl': 1})
elif loss == 'gear':
owner.pump(10, mask=1, gear=1)
elif loss == 'main':
owner.pump(10, main=0, mask=1)
elif loss == 'cancel':
owner.rx('Steering_Data_FD1', {'CcAslButtnCnclPress': 1})
elif loss == 'rx':
address, data, bus = owner.packer.make_can_msg('BrakeSysFeatures', 0, {'Veh_V_ActlBrk': 36., 'VehVActlBrk_D_Qf': 3})
bad = bytearray(data)
bad[0] ^= 1
assert not owner.safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, bad))
else:
assert owner.safety.safety_rx_hook(command(owner, active=False))
assert owner.safety.get_relay_malfunction()
stock_display(owner, relay=loss == 'relay')
owner.pump(10, mask=1)
if loss in ('eps_temp', 'lka', 'gear'):
assert owner.safety.aol_get_permission_mask() == 1
assert owner.safety.safety_tx_hook(command(owner, active=False)) == (loss != 'relay')
stock_display(owner, relay=loss == 'relay')
@pytest.mark.parametrize('word', (8, 66))
def test_other_ford_owners_keep_original_inactive_forwarding(word):
native = test_ford_aol.TestFordAolDriverIntent()
native.setUp()
native.reset(word)
native.arm()
bus = CanBus(fingerprint=gen_empty_fingerprint())
announce(native)
for active in (True, False):
if word == 8:
address, data, src = fordcan.create_lat_ctl_msg(native.packer, bus, active, 0., 0., 0., 0.)
packet = libsafety_py.make_CANPacket(address, src, data)
else:
packet = command(native, active=active)
assert native.safety.safety_tx_hook(packet)
for address in (0x18A, 0x3D8):
assert native.safety.safety_fwd_hook(2, address) == (-1 if active else 0)
@pytest.mark.parametrize('veto', ('eps', 'lka', 'gear'))
def test_transient_physical_veto_in_same_rx_batch_revokes_display_only(owner, veto):
acquire(owner)
assert owner.safety.safety_tx_hook(command(owner, active=False))
if veto == 'eps':
frames = [('EPAS_INFO', {'EPAS_Failure': value}) for value in (1, 0)]
elif veto == 'lka':
frames = [('Lane_Assist_Data3_FD1', {'LatCtlSte_D_Stat': value, 'LaActAvail_D_Actl': 3, 'LaActDeny_B_Actl': 0})
for value in (0, 1)]
elif owner.word & 256:
frames = [('BCM_Lamp_Stat_FD1', {'RvrseLghtOn_B_Stat': value}) for value in (1, 0)]
else:
frames = [('PowertrainData_10', {'TrnRng_D_Rq': value}) for value in (1, 3)]
for name, values in frames:
owner.rx(name, values)
assert owner.safety.aol_get_permission_mask() == 1
assert owner.safety.safety_tx_hook(command(owner, active=False))
stock_display(owner)
# Current retained AOL permission can reacquire only through accepted action.
assert owner.safety.safety_tx_hook(command(owner, active=True))
assert owner.safety.safety_fwd_hook(2, 0x18A) == -1
@@ -4,6 +4,8 @@ from tools.ci import run_vehicle_tests as runner
FAMILY_TARGETS = (
'opendbc_repo/opendbc/car/ford/tests/test_mache_display_handoff.py',
'opendbc_repo/opendbc/safety/tests/test_ford_mache_display.py',
'openpilot/starpilot/tests/test_ford_configuration_parity.py',
'openpilot/starpilot/tests/test_ford_configuration_admission.py',
'openpilot/starpilot/tests/test_ford_release_long_preferences.py',
+1 -1
View File
@@ -1 +1 @@
{"protected_inventory_sha256": "0a1b49ca7fd6da8459c05894e41fc6fde9fdf84ab29d22cb889ce3896141a5a3", "schema_version": 1}
{"protected_inventory_sha256": "52195ed2a6f589ece8c7572181b1bd593536183e045b9bc1d24ec456d247a91b", "schema_version": 1}
+4
View File
@@ -24,6 +24,8 @@ from tools.ci import lifecycle_shards, vehicle_pytest
# Representative production callers plus their source/native refusal cases.
# The emitted selected_test_ids record exact parametrized configurations.
AOL_LIFECYCLE_TARGETS = (
'opendbc_repo/opendbc/car/ford/tests/test_mache_display_handoff.py',
'opendbc_repo/opendbc/safety/tests/test_ford_mache_display.py',
'openpilot/starpilot/aol/tests/test_hkg_cancel_joined.py',
'openpilot/starpilot/aol/tests/test_gv70_two_panda_long.py',
'openpilot/starpilot/longitudinal/tests/test_gv70_stopping_parity.py',
@@ -197,6 +199,8 @@ DEBUG_SAFETY_CLASS_TARGETS = {
RELEASE_TARGETS = (
'opendbc_repo/opendbc/car/ford/tests/test_mache_display_handoff.py',
'opendbc_repo/opendbc/safety/tests/test_ford_mache_display.py',
"opendbc_repo/opendbc/safety/tests/test_toyota_prius_advert.py",
'opendbc_repo/opendbc/safety/tests/test_hyundai_gv60_long.py::TestGV60LongReleaseDenied',
'openpilot/starpilot/tests/test_ford_configuration_parity.py',