From de23ac008ebc6e5de13e293964eb0bcfa04a7d85 Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Mon, 14 Sep 2026 08:12:49 -0400 Subject: [PATCH] Ford: add live C0 distance toggle on comma four --- docs/ford_model_action_drive_test.md | 45 ++++- openpilot/common/params_keys.h | 1 + openpilot/selfdrive/controls/controlsd.py | 3 +- .../controls/lib/ford_model_action.py | 40 +++-- .../controls/tests/test_ford_c0_distance.py | 167 ++++++++++++++++++ .../tests/test_ford_controlsd_logging.py | 2 +- .../tests/test_ford_model_action_adapter.py | 4 +- .../tests/test_ford_model_action_selection.py | 6 +- .../ui/mici/layouts/settings/settings.py | 4 +- .../ui/sunnypilot/mici/layouts/settings.py | 3 + .../ui/sunnypilot/mici/layouts/toggles.py | 26 +++ .../selfdrive/controls/controlsd_ext.py | 8 + 12 files changed, 283 insertions(+), 26 deletions(-) create mode 100644 openpilot/selfdrive/controls/tests/test_ford_c0_distance.py create mode 100644 openpilot/selfdrive/ui/sunnypilot/mici/layouts/toggles.py diff --git a/docs/ford_model_action_drive_test.md b/docs/ford_model_action_drive_test.md index 37db552cc8..94d1561dae 100644 --- a/docs/ford_model_action_drive_test.md +++ b/docs/ford_model_action_drive_test.md @@ -1,16 +1,16 @@ # Ford selected-action drive-test branch -This v11 controller restores [curvature-derived C0](ford_curvature_c0_v8.md) and retains direct C0/C1 requests +This v12 controller restores [curvature-derived C0](ford_curvature_c0_v8.md) and retains direct C0/C1 requests and [continuous C1 PI feedback](ford_c1_minimal_pi.md) with **P=0.50 and I=0.25**. -Only integrated tracking error accumulates correction; C0/C1 reflect the current bounded request. C0 is now a 7 m circular arc from selected desired curvature. +Only integrated tracking error accumulates correction; C0/C1 reflect the current bounded request. C0 defaults to a 7 m circular arc from selected desired curvature. An on-device toggle can instead use max(7 m, speed × 1 second). [Base C1 overflow allocation to C0](ford_c1_overflow.md) remains. It is selectable on **any Ford CAN FD vehicle** through the existing persistent, default-off Sunnylink toggle. Offline checks establish software behavior; physical tracking, turn-exit behavior and closed-loop stability remain unvalidated. -V11 restores the v9 command law after the model-path C0 trial in `5db3e3c9a`. +V12 retains the v11/v9 command law by default after the model-path C0 trial in `5db3e3c9a`. Both base commands use selected, upstream-limited desired curvature. The gains remain P=0.50 and I=0.25, and PSCM `LimitReached` handling is unchanged. The separate offline experiment that ignores the reached-limit integration block is not included. @@ -27,7 +27,7 @@ offline experiment that ignores the reached-limit integration block is not inclu The startup event `Ford path controller selected` should report `FordModelActionController`. Periodic `Ford C2-free path tracking` events -identify **`hypothesis=model-action-curvature-c0-direct-pi-v11`**. They report desired and measured +identify **`hypothesis=model-action-curvature-c0-distance-pi-v12`**. They report desired and measured curvature, base heading, proportional and accumulated correction, applied heading, feedback timing and driver/PSCM gating. `proportional_gain=0.5` and `integral_gain=0.25` identify the trial. `offset_overflow` reports the extra C0 @@ -42,6 +42,41 @@ a custom controller. The observer toggle is no longer exposed. The experiment only runs on Ford CAN FD vehicles; legacy Ford uses upstream control as well. See [toggle-off validation](ford_upstream_fallback.md). +## C0 distance toggle on comma four + +With the experimental Ford controller enabled, open **Settings → toggles → C0: 1 second**. +The toggle is visible for Ford CAN FD vehicles and can be changed while disengaged. + +- **Off (default):** C0 uses a fixed 7 m arc. +- **On:** C0 uses a distance of max(7 m, speed × 1 second), matching the base C1 distance. + +Disengage assistance, change the toggle, and remain disengaged for at least three seconds +before reengaging. This setting uses the existing three-second runtime parameter refresh; +**no ignition cycle or controlsd restart is required**. Engaged or paused MADS and stale +engagement messages prevent applying a change. A mode change resets the PI correction and +adapter timestamps. Reapplying the same value does not reset anything. + +The persistent parameter is `FordC0TimeBased`. It cannot enable the experimental controller +by itself. The existing Sunnylink controller-selection toggle still requires an onroad cycle. +C1, the gains, the 7 m heading-overflow allocation, the upstream reference limits and the CAN +field bounds are unchanged. Below 7 m/s (about 15.7 mph), both distance modes are identical. +At 20/30/60 mph the enabled distance is approximately 8.9/13.4/26.8 m, respectively; C0 can +therefore be substantially larger, especially at higher speeds. Its release still follows the +current selected curvature immediately, with no additional slew. + +The `Ford C0 distance changed` event records an applied switch. Periodic tracking events +include `c0_time_based` and the actual `offset_distance` in meters, including the default mode. +Offline checks verify selection, runtime switching, resets, unchanged C1 and CAN encoding; +they do not establish which distance the PSCM follows better. + +Validation on 2026-09-14: 410 tests and 25 subtests passed, plus Ruff and the local comma four +UI construction/write/refresh/visibility/render check. The 54,146-cycle maneuver-route replay +(`84865544361f55cb/0000011c--99f4537696`) matched `775012167` exactly with the new toggle off. +With it on, C0 changed in 35,668 cycles (maximum difference 0.74 m), while C1 and accumulated +correction remained identical on the same recorded motion. The two comparisons completed +216,584 controller updates and CAN round trips. No vehicle build, installation or road test +was performed for this change. + ## Wiring and validation `controlsd` supplies the selected, upstream-limited desired curvature and the @@ -57,7 +92,7 @@ combined feedforward/P/I amplitude envelope. There is no C0 confirmation threshold or remembered turn direction. Zero error removes P and holds I; it does not trigger a release. Final command limits still apply. -C0 starts with the 7 m circular arc of selected desired curvature. It does not +C0 starts with the selected-distance circular arc of selected desired curvature. It does not add independent live model-path position or heading. Valid model geometry is still required as a health gate. When the raw base heading exceeds ±0.5 rad, C0 additionally receives 7 m times the clipped-away heading. diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index b41f396a09..896d61bcc1 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -239,6 +239,7 @@ inline static std::unordered_map keys = { // sunnypilot car specific params {"FordPscmObserver", {PERSISTENT | BACKUP, BOOL, "0"}}, {"FordModelActionController", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"FordC0TimeBased", {PERSISTENT | BACKUP, BOOL, "0"}}, {"HyundaiLongitudinalTuning", {PERSISTENT | BACKUP, INT, "0"}}, {"SubaruStopAndGo", {PERSISTENT | BACKUP, BOOL, "0"}}, {"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}}, diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index ca70c80117..62316f943f 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -55,7 +55,8 @@ class Controls(ControlsExt): self.steer_limited_by_safety = False self.curvature = 0.0 self.desired_curvature = 0.0 - self.ford_path_controller = select_model_action_controller(self.CP, self.params.get_bool("FordModelActionController")) + self.ford_path_controller = select_model_action_controller(self.CP, self.params.get_bool("FordModelActionController"), + c0_time_based=self.params.get_bool("FordC0TimeBased")) self.ford_model_action = isinstance(self.ford_path_controller, FordModelActionController) if self.CP.brand == "ford": cloudlog.event("Ford path controller selected", diff --git a/openpilot/selfdrive/controls/lib/ford_model_action.py b/openpilot/selfdrive/controls/lib/ford_model_action.py index af6e28e06e..811a2b9458 100644 --- a/openpilot/selfdrive/controls/lib/ford_model_action.py +++ b/openpilot/selfdrive/controls/lib/ford_model_action.py @@ -1,6 +1,7 @@ """Opt-in Ford C2-free model mapping with measured-curvature PI feedback. -C0 samples a desired-curvature arc at 7 m, including base-heading overflow. C1 +C0 samples a desired-curvature arc at 7 m, optionally max(7 m, v*1s), +including base-heading overflow. C1 combines the selected curvature's heading with proportional and integrated tracking error. Reference distance and gains are explicit trial choices. Commands use the current bounded request without an additional C0/C1 slew. @@ -34,8 +35,8 @@ def _finite(*values): return False -def encode_model_action(model, desired_curvature, speed): - """Encode a 7 m circular-arc offset and max(7, v*1s)*selected curvature. +def encode_model_action(model, desired_curvature, speed, *, c0_time_based=False): + """Encode a circular-arc offset and max(7, v*1s)*selected curvature. The arc starts at zero lateral position and heading. Original model geometry remains a health gate; selected curvature supplies both path commands. @@ -49,9 +50,10 @@ def encode_model_action(model, desired_curvature, speed): if path is None or not all(_finite(*values) for values in path): return FordPath() # (1-cos(S*k))/k, using sinc to avoid cancellation near zero curvature. - half_heading = .5*OFFSET_STATION_M*desired_curvature + distance = max(OFFSET_STATION_M, speed*HEADING_TIME_S) if c0_time_based else OFFSET_STATION_M + half_heading = .5*distance*desired_curvature sinc = math.sin(half_heading)/half_heading if half_heading else 1. - c0 = .5*desired_curvature*OFFSET_STATION_M**2*sinc**2 + c0 = .5*desired_curvature*distance**2*sinc**2 c1 = max(OFFSET_STATION_M, speed*HEADING_TIME_S)*desired_curvature return FordPath(True, c0, c1, 0., 0.) if _finite(c0, c1) else FordPath() @@ -61,12 +63,13 @@ class ModelActionController: Freshness, measurement cadence and driver/PSCM arbitration belong to the caller. """ - __slots__ = ('c0', 'c1', 'correction', 'proportional_gain', 'integral_gain', 'proportional', 'feedback_curvature') + __slots__ = ('c0', 'c1', 'correction', 'proportional_gain', 'integral_gain', 'proportional', 'feedback_curvature', 'c0_time_based') - def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN): + def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False): if not _finite(proportional_gain, integral_gain) or min(proportional_gain, integral_gain) < 0.: raise ValueError('PI gains must be finite and nonnegative') self.proportional_gain, self.integral_gain = float(proportional_gain), float(integral_gain) + self.c0_time_based = bool(c0_time_based) self.reset() def reset(self): @@ -80,7 +83,7 @@ class ModelActionController: or not 0. <= feedback_dt <= .15 or abs(current_curvature) > 1. or abs(reference) > 1.): self.reset() return FordPath() - target = encode_model_action(model, desired_curvature, speed) + target = encode_model_action(model, desired_curvature, speed, c0_time_based=self.c0_time_based) if not target.valid: self.reset() return FordPath() @@ -126,15 +129,24 @@ class FordModelActionController: clears the correction. Fresh PSCM limits only inhibit outward integration; neither a limit nor a repeated measurement freezes the model request. """ - def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN): - self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain) - self.hypothesis = 'model-action-curvature-c0-direct-pi-v11' + def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False): + self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain, c0_time_based=c0_time_based) + self.hypothesis = 'model-action-curvature-c0-distance-pi-v12' self.reset() + def set_c0_time_based(self, enabled, *, lateral_engaged): + """Apply a distance change only after lateral assistance is disengaged.""" + if lateral_engaged or self.core.c0_time_based == bool(enabled): + return False + self.core.c0_time_based = bool(enabled) + self.reset('c0_distance_changed') + return True + def reset(self, status='inactive'): self.core.reset() self.last_time = self.last_measurement_time = self.last_model_time = None self.diagnostics = {'status': status, 'hypothesis': self.hypothesis, + 'c0_time_based': self.core.c0_time_based, 'calibration_approved': CALIBRATION_APPROVED, 'command': (0., 0., 0., 0.)} def update(self, model, desired_curvature, *, current_curvature, yaw_rate, speed, now, measurement_time, model_time, @@ -179,6 +191,8 @@ class FordModelActionController: raw_heading = max(OFFSET_STATION_M, speed*HEADING_TIME_S)*desired_curvature base_heading = float(np.clip(raw_heading, -.5, .5)) self.diagnostics = {'status': 'active', 'hypothesis': self.hypothesis, + 'c0_time_based': self.core.c0_time_based, + 'offset_distance': max(OFFSET_STATION_M, speed*HEADING_TIME_S) if self.core.c0_time_based else OFFSET_STATION_M, 'calibration_approved': CALIBRATION_APPROVED, 'desired_curvature': desired_curvature, 'model_age': now - model_time, 'measurement_age': now - measurement_time, 'reference_age': now - reference_time, 'dt': dt, 'offset_request': self.core.c0, 'heading_request': self.core.c1, @@ -194,9 +208,9 @@ class FordModelActionController: return command -def select_model_action_controller(CP, enabled): +def select_model_action_controller(CP, enabled, *, c0_time_based=False): """Only opt-in Ford CAN FD vehicles override upstream curvature control.""" compatible = CP.brand == 'ford' and CP.flags & FordFlags.CANFD if enabled and compatible: - return FordModelActionController(proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN) + return FordModelActionController(proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, c0_time_based=c0_time_based) return None diff --git a/openpilot/selfdrive/controls/tests/test_ford_c0_distance.py b/openpilot/selfdrive/controls/tests/test_ford_c0_distance.py new file mode 100644 index 0000000000..4ec10db999 --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_ford_c0_distance.py @@ -0,0 +1,167 @@ +"""C0 distance mapping and live setting changes, without vehicle hardware.""" +import ast +import math +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest + +from opendbc.can import CANPacker, CANParser +from opendbc.car.ford.fordcan import CanBus, create_lat_ctl2_msg +from openpilot.common.params import Params, ParamKeyFlag, ParamKeyType +from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController, ModelActionController, encode_model_action +from openpilot.selfdrive.controls.lib.ford_path import FordPath +from openpilot.selfdrive.controls.tests.test_ford_model_action import straight +from openpilot.selfdrive.controls.tests.test_ford_model_action_adapter import _method, update +from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import startup + + +@pytest.mark.parametrize('speed', [.3, 3., 7., 8.94, 13.41, 26.82, 55.]) +@pytest.mark.parametrize('curvature', [-.03, -1e-9, 0., 1e-9, .03]) +def test_arc_distance_changes_only_c0_above_seven_meters_per_second(speed, curvature): + fixed = encode_model_action(straight(), curvature, speed) + timed = encode_model_action(straight(), curvature, speed, c0_time_based=True) + distance = max(7., speed) + # Independent small-angle expansion avoids cancellation at nearly zero k. + expected = (.5*curvature*distance**2 if abs(curvature) < 1e-6 else (1-math.cos(curvature*distance))/curvature) + assert timed.path_offset == pytest.approx(expected) + assert timed.path_angle == fixed.path_angle + assert timed.curvature == timed.curvature_rate == 0. + if speed <= 7.: + assert timed == fixed + + +@pytest.mark.parametrize('enabled', [False, True]) +def test_reversal_and_zero_request_remain_immediate_on_the_wire(enabled): + core = ModelActionController(c0_time_based=enabled) + packer = CANPacker('ford_lincoln_base_pt') + parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], 0) + bus = CanBus(fingerprint={0: {}}) + for i, k in enumerate([.01]*100+[-.01, 0.]): + command = core.update(straight(), k, current_curvature=k, speed=20., dt=.01) + packet = create_lat_ctl2_msg(packer, bus, 2, -command.path_offset, -command.path_angle, 0., 0., i % 16) + parser.update([i*10_000_000, [packet]]) + wire = parser.vl['LateralMotionControl2'] + assert wire['LatCtlPathOffst_L_Actl'] == pytest.approx(-command.path_offset) + assert wire['LatCtlPath_An_Actl'] == pytest.approx(-command.path_angle) + assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0. + assert command.path_offset*k >= 0. and command.path_angle*k >= 0. + if k == 0.: + assert command == FordPath(True, 0., 0., 0., 0.) + + +def test_same_feedback_produces_identical_c1_and_integral_in_both_modes(): + cores = [ModelActionController(c0_time_based=mode) for mode in (False, True)] + model = straight() + for i in range(2000): + k = .03*math.sin(i*.03) + kwargs = {'current_curvature': .02*math.sin(i*.03-.5), 'speed': 20., 'dt': .01, + 'feedback_enabled': i % 77 != 0, 'pscm_limited': i % 3 == 0} + outputs = [core.update(model, k, **kwargs) for core in cores] + assert outputs[0].path_angle == outputs[1].path_angle + assert cores[0].correction == cores[1].correction + assert cores[0].proportional == cores[1].proportional + for out in outputs: + assert abs(out.path_offset) <= 5.110001 and abs(out.path_angle) <= .500001 + + +@pytest.mark.parametrize('initial', [False, True]) +def test_change_resets_feedback_and_timestamps_but_a_noop_does_not(initial): + controller = FordModelActionController(c0_time_based=initial) + for i in range(20): + update(controller, 1.+i*.01, current_curvature=0.) + assert controller.core.correction > 0. + before = controller.diagnostics.copy() + assert not controller.set_c0_time_based(not initial, lateral_engaged=True) + assert controller.diagnostics == before and controller.core.c0_time_based == initial + assert not controller.set_c0_time_based(initial, lateral_engaged=False) + assert controller.diagnostics == before + assert controller.set_c0_time_based(not initial, lateral_engaged=False) + assert controller.core.correction == controller.core.proportional == controller.core.c0 == controller.core.c1 == 0. + assert controller.last_time is controller.last_measurement_time is controller.last_model_time is None + assert controller.diagnostics['status'] == 'c0_distance_changed' + assert controller.diagnostics['c0_time_based'] == (not initial) + assert update(controller, 10.) == update(FordModelActionController(c0_time_based=not initial), 10.) + assert controller.diagnostics['offset_distance'] == (7. if initial else 20.) + + +@pytest.fixture +def runtime(tmp_path): + params = Params(str(tmp_path)) + params.put_bool('FordModelActionController', True, block=True) + controls = startup(params=params) + controls.CP.lateralTuning = SimpleNamespace(which=lambda: 'angle') + controls._param_update_time = 0. + controls.blinker_pause_lateral = SimpleNamespace(get_params=lambda: None) + clock = SimpleNamespace(now=4., monotonic=lambda: clock.now) + events = [] + filename = Path(__file__).resolve().parents[3]/'sunnypilot/selfdrive/controls/controlsd_ext.py' + method = _method(filename, 'ControlsExt', 'get_params_sp') + env = {'time': clock, 'PARAMS_UPDATE_PERIOD': 3., 'messaging': SimpleNamespace(SubMaster=object), + 'cloudlog': SimpleNamespace(event=lambda *args, **kwargs: events.append((args, kwargs)))} + exec(compile(ast.Module(body=[method], type_ignores=[]), str(filename), 'exec'), env) + controls.refresh = lambda sm: env['get_params_sp'](controls, sm) + return controls, params, clock, events + + +class EngagementMessages(dict): + healthy = True + + def all_checks(self, services): + return self.healthy and all(service in self for service in services) + + +@pytest.mark.parametrize('mads_available', [False, True]) +def test_running_process_defers_changes_until_disengaged_and_honors_poll_period(runtime, mads_available): + controls, params, clock, events = runtime + mads = SimpleNamespace(available=mads_available, enabled=True, active=False) # includes a paused MADS state + standard = SimpleNamespace(enabled=True, active=False) + sm = EngagementMessages(selfdriveStateSP=SimpleNamespace(mads=mads), selfdriveState=standard) + params.put_bool('FordC0TimeBased', True, block=True) + controller = controls.ford_path_controller + update(controller, current_curvature=0.) + controls.refresh(sm) + assert not controller.core.c0_time_based + mads.enabled = standard.enabled = False + clock.now = 5. + controls.refresh(sm) + assert not controller.core.c0_time_based # next scheduled refresh has not run yet + clock.now = 7.01 + controls.refresh(sm) + assert controller.core.c0_time_based and controller.last_time is None + assert controls.ford_path_controller is controller # same controlsd/controller instance + assert len(events) == 1 + params.put_bool('FordC0TimeBased', False, block=True) + sm.healthy = False + clock.now += 3.01 + controls.refresh(sm) + assert controller.core.c0_time_based # stale engagement data cannot permit a swap + sm.healthy = True + clock.now += 3.01 + controls.refresh(sm) + assert not controller.core.c0_time_based and len(events) == 2 + + +def test_setting_is_persistent_default_off_and_cannot_enable_custom_control(tmp_path): + params = Params(str(tmp_path)) + assert params.get_default_value('FordC0TimeBased') is False + assert params.get_type('FordC0TimeBased') == ParamKeyType.BOOL + for flag in (ParamKeyFlag.PERSISTENT, ParamKeyFlag.BACKUP): + assert b'FordC0TimeBased' in params.all_keys(flag) + params.put_bool('FordC0TimeBased', True, block=True) + assert startup(params=params).ford_path_controller is None + params.put_bool('FordModelActionController', True, block=True) + assert startup(params=params).ford_path_controller.core.c0_time_based + params.clear_all(ParamKeyFlag.CLEAR_ON_MANAGER_START) + assert Params(str(tmp_path)).get_bool('FordC0TimeBased') + + +@pytest.mark.parametrize('speed', [3., 10., 20., 35., 55.]) +def test_timed_mode_retains_bounds_at_extreme_and_nonfinite_requests(speed): + core = ModelActionController(c0_time_based=True) + for curvature in np.linspace(-1., 1., 101): + out = core.update(straight(), curvature, current_curvature=0., speed=speed, dt=.01) + assert out.valid and abs(out.path_offset) <= 5.110001 and abs(out.path_angle) <= .500001 + for invalid in (math.nan, math.inf, -math.inf): + assert core.update(straight(), invalid, current_curvature=0., speed=speed, dt=.01) == FordPath() diff --git a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py index c3931fab50..c0f7c48a95 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py +++ b/openpilot/selfdrive/controls/tests/test_ford_controlsd_logging.py @@ -53,7 +53,7 @@ class TestFordControlsLogging(unittest.TestCase): controls = SimpleNamespace(ford_path_controller=controller, desired_curvature=.03, curvature=.015, sm=SimpleNamespace(logMonoTime={'modelV2': 123456789, 'carState': 123450000})) record = self.emit_controls_event('Ford C2-free path tracking', controls) - self.assertEqual(record['hypothesis'], 'model-action-curvature-c0-direct-pi-v11') + self.assertEqual(record['hypothesis'], 'model-action-curvature-c0-distance-pi-v12') self.assertIs(record['calibration_approved'], False) self.assertEqual(record['command'][2:], [0., 0.]) self.assertEqual(record['status'], controller.diagnostics['status']) diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py index 8de688b3d6..b9b722ba13 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py @@ -355,7 +355,7 @@ def test_continuous_pi_reversal_through_selected_limited_request_and_actual_can( assert wire['LatCtlPath_No_Cs'] == calculate_lat_ctl2_checksum(2, frame % 16, packet[1]) if frame == 199: assert sign*core.correction < 0. if same_turn else sign*core.correction > 0. - assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-direct-pi-v11' + assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-distance-pi-v12' if same_turn: assert controls.desired_curvature == pytest.approx(sign*.01) assert sign*controls.ford_path.path_angle >= speed*.01 # No old unwind correction left below the new base. @@ -473,7 +473,7 @@ def test_toggle_off_preserves_upstream_actuators_and_can(pipeline, fingerprint, call, publication = pipeline settings = {'FordModelActionController': False, 'FordPscmObserver': observer} flags = CAR(fingerprint).config.flags - controls = startup(car_params(carFingerprint=fingerprint, flags=flags), SimpleNamespace(get_bool=settings.__getitem__)) + controls = startup(car_params(carFingerprint=fingerprint, flags=flags), SimpleNamespace(get_bool=lambda key: settings.get(key, False))) assert controls.ford_path_controller is None sm = Subscriptions(False) controls.sm, controls.desired_curvature, controls.curvature = sm, .004, 0. diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py index 0c74829f3c..6c501c194f 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py @@ -43,12 +43,12 @@ def startup(cp=None, params=None): @pytest.mark.parametrize('fingerprint', [*CANFD_CARS, 'FORD_FUTURE_CANFD']) def test_actual_startup_priority(candidate, observer, fingerprint): settings = {'FordModelActionController': candidate, 'FordPscmObserver': observer} - selected = startup(car_params(carFingerprint=fingerprint), params=SimpleNamespace(get_bool=settings.__getitem__)) + selected = startup(car_params(carFingerprint=fingerprint), params=SimpleNamespace(get_bool=lambda key: settings.get(key, False))) if candidate: assert type(selected.ford_path_controller) is FordModelActionController assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .50 assert selected.ford_path_controller.core.integral_gain == C1_INTEGRAL_GAIN == .25 - assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-direct-pi-v11' + assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-distance-pi-v12' else: assert selected.ford_path_controller is None assert selected.ford_model_action == candidate @@ -59,7 +59,7 @@ def test_actual_startup_priority(candidate, observer, fingerprint): @pytest.mark.parametrize('observer', [False, True]) def test_other_vehicles_always_use_upstream(overrides, observer): settings = {'FordModelActionController': False, 'FordPscmObserver': observer} - params = SimpleNamespace(get_bool=settings.__getitem__) + params = SimpleNamespace(get_bool=lambda key: settings.get(key, False)) before = startup(car_params(**overrides), params) settings['FordModelActionController'] = True after = startup(car_params(**overrides), params) diff --git a/openpilot/selfdrive/ui/mici/layouts/settings/settings.py b/openpilot/selfdrive/ui/mici/layouts/settings/settings.py index eb7789cba9..d06e0a7759 100644 --- a/openpilot/selfdrive/ui/mici/layouts/settings/settings.py +++ b/openpilot/selfdrive/ui/mici/layouts/settings/settings.py @@ -16,11 +16,13 @@ class SettingsBigButton(BigButton): class SettingsLayout(NavScroller): + toggles_layout = TogglesLayoutMici + def __init__(self): super().__init__() self._params = Params() - toggles_panel = TogglesLayoutMici() + toggles_panel = self.toggles_layout() toggles_btn = SettingsBigButton("toggles", "", gui_app.texture("icons_mici/settings.png", 64, 64)) toggles_btn.set_click_callback(lambda: gui_app.push_widget(toggles_panel)) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py index 4c0eba41ea..0944cf05e3 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/settings.py @@ -11,6 +11,7 @@ from openpilot.selfdrive.ui.mici.widgets.button import BigCircleButton from openpilot.selfdrive.ui.mici.widgets.dialog import BigConfirmationDialog, BigDialog from openpilot.selfdrive.ui.sunnypilot.mici.layouts.sunnylink import SunnylinkLayoutMici from openpilot.selfdrive.ui.sunnypilot.mici.layouts.models import ModelsLayoutMici +from openpilot.selfdrive.ui.sunnypilot.mici.layouts.toggles import TogglesLayoutMiciSP from openpilot.selfdrive.ui.ui_state import ui_state from openpilot.system.ui.lib.application import gui_app, FontWeight from openpilot.system.ui.lib.multilang import tr @@ -30,6 +31,8 @@ class SunnylinkBigButton(SettingsBigButton): class SettingsLayoutSP(OP.SettingsLayout): + toggles_layout = TogglesLayoutMiciSP + def __init__(self): OP.SettingsLayout.__init__(self) diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/toggles.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/toggles.py new file mode 100644 index 0000000000..b6b7f1bebc --- /dev/null +++ b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/toggles.py @@ -0,0 +1,26 @@ +from opendbc.car.ford.values import FordFlags + +from openpilot.selfdrive.ui.mici.layouts.settings.toggles import TogglesLayoutMici +from openpilot.selfdrive.ui.mici.widgets.button import BigParamControl, GreyBigButton +from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.system.ui.lib.multilang import tr + + +class TogglesLayoutMiciSP(TogglesLayoutMici): + def __init__(self): + super().__init__() + self._ford_c0_toggle = BigParamControl(tr('C0: 1 second'), 'FordC0TimeBased') + self._ford_c0_help = GreyBigButton('', tr('off: fixed 7 m\non: 1 second, min 7 m\ndisengage 3 s to apply\nno ignition cycle')) + self._ford_c0_toggle.set_enabled(lambda: not ui_state.engaged) + self._scroller.add_widgets([self._ford_c0_toggle, self._ford_c0_help]) + self._refresh_toggles += (('FordC0TimeBased', self._ford_c0_toggle),) + self._ford_c0_toggle.set_visible(False) + self._ford_c0_help.set_visible(False) + + def _update_toggles(self): + super()._update_toggles() + cp = ui_state.CP + visible = bool(cp is not None and cp.brand == 'ford' and cp.flags & FordFlags.CANFD + and ui_state.params.get_bool('FordModelActionController')) + self._ford_c0_toggle.set_visible(visible) + self._ford_c0_help.set_visible(visible) diff --git a/openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py b/openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py index f71f18a15c..7919ef5e6a 100644 --- a/openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -51,6 +51,14 @@ class ControlsExt(ModelStateBase): if time.monotonic() - self._param_update_time > PARAMS_UPDATE_PERIOD: self.blinker_pause_lateral.get_params() + if getattr(self, 'ford_model_action', False) and sm.all_checks(['selfdriveState', 'selfdriveStateSP']): + mads = sm['selfdriveStateSP'].mads + # Use the engagement state, not a temporary pause from blinkers or a + # standstill/fault gate, so a pause cannot swap the command mapping. + lateral_engaged = mads.enabled if mads.available else sm['selfdriveState'].enabled + if self.ford_path_controller.set_c0_time_based(self.params.get_bool('FordC0TimeBased'), lateral_engaged=lateral_engaged): + cloudlog.event('Ford C0 distance changed', c0_time_based=self.ford_path_controller.core.c0_time_based) + if self.CP.lateralTuning.which() == 'torque': self.lat_delay = get_lat_delay(self.params, sm["lateralDelay"].lateralDelay)