From 3b657f6de9b299ea002db739c2c84063dda4f8bd Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Thu, 17 Sep 2026 20:22:05 -0400 Subject: [PATCH] Ford: gate coordinated C0/C1 encoder behind sunnylink trial toggle --- SConstruct | 1 + docs/ford_joint_control_trial.md | 58 +++ openpilot/common/params_keys.h | 1 + openpilot/selfdrive/car/card.py | 20 +- openpilot/selfdrive/car/ford_joint_control.py | 148 ++++++ .../car/tests/test_ford_joint_control.py | 253 ++++++++++ openpilot/selfdrive/controls/controlsd.py | 4 +- .../controls/lib/ford_joint/SConscript | 3 + .../controls/lib/ford_joint/__init__.py | 0 .../controls/lib/ford_joint/angle.py | 31 ++ .../controls/lib/ford_joint/calibration.json | 475 ++++++++++++++++++ .../controls/lib/ford_joint/encoder.cc | 100 ++++ .../controls/lib/ford_joint/encoder.py | 112 +++++ .../controls/lib/ford_joint/inverse.py | 80 +++ .../controls/lib/ford_joint/model.py | 179 +++++++ .../controls/lib/ford_model_action.py | 19 +- .../ui/sunnypilot/mici/layouts/toggles.py | 3 +- .../sunnypilot/sunnylink/settings_ui.json | 23 + .../settings_ui_src/pages/vehicle.yaml | 14 + .../sunnylink/tests/test_settings_schema.py | 10 + 20 files changed, 1525 insertions(+), 9 deletions(-) create mode 100644 docs/ford_joint_control_trial.md create mode 100644 openpilot/selfdrive/car/ford_joint_control.py create mode 100644 openpilot/selfdrive/car/tests/test_ford_joint_control.py create mode 100644 openpilot/selfdrive/controls/lib/ford_joint/SConscript create mode 100644 openpilot/selfdrive/controls/lib/ford_joint/__init__.py create mode 100644 openpilot/selfdrive/controls/lib/ford_joint/angle.py create mode 100644 openpilot/selfdrive/controls/lib/ford_joint/calibration.json create mode 100644 openpilot/selfdrive/controls/lib/ford_joint/encoder.cc create mode 100644 openpilot/selfdrive/controls/lib/ford_joint/encoder.py create mode 100644 openpilot/selfdrive/controls/lib/ford_joint/inverse.py create mode 100644 openpilot/selfdrive/controls/lib/ford_joint/model.py diff --git a/SConstruct b/SConstruct index 4e9dedd947..8ed609841b 100644 --- a/SConstruct +++ b/SConstruct @@ -288,6 +288,7 @@ if arch == "comma_arm64": SConscript([ 'openpilot/selfdrive/pandad/SConscript', 'openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript', + 'openpilot/selfdrive/controls/lib/ford_joint/SConscript', 'openpilot/selfdrive/locationd/SConscript', 'openpilot/selfdrive/modeld/SConscript', 'openpilot/selfdrive/ui/SConscript', diff --git a/docs/ford_joint_control_trial.md b/docs/ford_joint_control_trial.md new file mode 100644 index 0000000000..a5283264fc --- /dev/null +++ b/docs/ford_joint_control_trial.md @@ -0,0 +1,58 @@ +# Coordinated C0/C1 steering trial (v24) + +This default-off trial turns the normal action-derived desired steering angle into a jointly selected C0/C1 pair. It estimates the held fields and curvature filter of an older Ford PSCM and chooses one command whose predicted next-update error is no worse than a C1-anchored alternative. Among those candidates it minimizes error over the complete return to the steady pair, including the filter tail. It does not retain a future command plan or classify turns into maneuver states. + +## Select and revert + +In sunnylink → Vehicle → Ford Settings, while offroad: + +- **Selected-Action Path Tracking (Experimental): ON** +- **Model Geometry Reference: OFF** +- **Coordinated C0/C1 Steering (Experimental): ON** + +Apply with an offroad-to-onroad cycle. The new `FordPscmJointControl` parameter defaults to false. Turning it off restores v23 selected-action control. Turning the master Selected-Action setting off restores upstream Ford control, regardless of this stored setting. Geometry mode and Joystick Debug Mode keep their existing paths. C0 distance selection does not affect this trial. + +The integration accepts Ford CAN FD platforms, following the existing master gate. Compatibility with each PSCM calibration is not established. + +## Live integration + +`controlsd` retains the normal action source, curvature limits, vehicle-model conversion, input freshness checks and steering-angle target. With the trial selected, its existing Ford adapter is a validity carrier with zero P/I gains; it supplies no additional path correction. `card` performs the joint allocation immediately before the existing Ford CAN packer. It negates the encoder's CAN-coordinate output into the OP path interface; the packer negates it back on transmission. + +C2/C3 remain zero. Existing field bounds, mode 2 and immediate ramp remain unchanged. The observer advances at an estimated 8 ms firmware cadence using the decoded packets actually queued to panda. Queueing is not a PSCM acknowledgment. It retains held/filter estimates while disengaged or overridden and applies the recovered inactive slew to mode-0 packets. A fresh limitReached status does not freeze the target; driver override or denial inhibits this trial's lateral command. Longitudinal controls are untouched. + +A command-history gap over 100 ms, nonmonotonic clock or invalid prediction latches the trial inactive until the next onroad process start. The normal steering-fault alert reports the latched fault. There is no mid-turn switch to another controller. Diagnostics use `Ford joint path tracking` / `ford-joint-v24` and distinguish requested angle, reachable angle, estimated held state, proposed pair and packed pair. + +## What the estimate does and does not know + +Packaged numerical calibration comes from ML3V-14D003-BD / ML34-14D007-EDL. No executable firmware or private firmware image is shipped. Equivalence to the Lightning's RL38 firmware remains unverified. Internal integral is assumed initially zero and frozen; interaction is nominally 1.0; acceleration uses measured yaw × speed (no measured bank input); vehicle geometry comes from CarParams. These are explicit assumptions, not measured PSCM RAM. The model's 3 m/s² acceleration allowance is retained. It estimates an internal angle target before subsequent PSCM filtering/torque stages, not the future wheel angle. + +## Validation + +The production Python/C++ port reproduces every C0/C1 sample of the audited prototype in seven frozen windows from routes 166 and 16a. Held fields and filtered curvature also match; the largest angle-stage numerical difference is below 6e-14 degrees. + +A separate replay executes the actual 100 Hz adapter and Ford CAN packer with the reconstructed response ticking at 125 Hz. Mean internal-target error, degrees: + +| Window | Audited prototype | Actual 100 Hz adapter | +|---|---:|---:| +| 16a wobble 1 | 0.520 | 0.521 | +| 16a wobble 2 | 0.240 | 0.248 | +| 16a exit | 3.509 | 3.503 | +| 16a turn | 9.762 | 9.981 | +| 166 wobble 1 | 3.491 | 3.545 | +| 166 wobble 2 | 0.530 | 0.587 | +| 166 good left | 4.124 | 4.222 | + +These are frozen measurements and nominal active-mode reconstruction, not observed improvements on the truck. The actual adapter made 12,031 packed command updates without a latched fault. The slowest per-window 99th-percentile cycle was 0.95 ms on the development Mac; comma hardware timing is unmeasured. Firmware-rate/calibration uncertainty remains material. + +The focused suite exercises default-off selection, rollback, zero second PI, CAN signs/bounds/cadence, inactive state retention, interventions, limitReached, stale/nonfinite inputs, timing faults, real card hooks, parameter/schema registration, and inverse/forward numerical agreement. Run: + +```sh +python -m pytest openpilot/selfdrive/car/tests/test_ford_joint_control.py \ + openpilot/selfdrive/car/tests/test_ford_pscm_status.py \ + openpilot/selfdrive/controls/tests/test_ford_model_action*.py \ + openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py +``` + +Build the native kernel through SCons. On the development checkout, its real SConscript and native parameter library were compiled successfully. A complete root build could not run because the checkout lacks the msgq/rednose SCons tool submodules. No on-device build or road validation is claimed. + +The trial's question is whether coordinating both channels preserves entry while reducing unnecessary correction during release. Offline results justify the opt-in comparison; they cannot establish smoothness or closed-loop stability. diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 7e4f5d99aa..13acf4afd7 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -241,6 +241,7 @@ inline static std::unordered_map keys = { {"FordModelActionController", {PERSISTENT | BACKUP, BOOL, "0"}}, {"FordC0TimeBased", {PERSISTENT | BACKUP, BOOL, "0"}}, {"FordGeometryReference", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"FordPscmJointControl", {PERSISTENT | BACKUP, BOOL, "0"}}, {"HyundaiLongitudinalTuning", {PERSISTENT | BACKUP, INT, "0"}}, {"SubaruStopAndGo", {PERSISTENT | BACKUP, BOOL, "0"}}, {"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}}, diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index 2182dd57b3..62453b22f4 100755 --- a/openpilot/selfdrive/car/card.py +++ b/openpilot/selfdrive/car/card.py @@ -22,6 +22,7 @@ from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp from openpilot.selfdrive.car.cruise import VCruiseHelper from openpilot.selfdrive.car.helpers import convert_carControlSP, convert_to_capnp from openpilot.selfdrive.car.ford_pscm_status import populate_ford_pscm_status +from openpilot.selfdrive.car.ford_joint_control import select_joint_control from openpilot.sunnypilot.mads.helpers import set_alternative_experience, set_car_specific_params from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces @@ -124,6 +125,7 @@ class Car: self.RI = RI self.CP.alternativeExperience = 0 + self.ford_joint_control = select_joint_control(self.CP, self.params) # mads set_alternative_experience(self.CP, self.CP_SP, self.params) set_car_specific_params(self.CP, self.CP_SP, self.params) @@ -200,6 +202,10 @@ class Car: CS, CS_SP = self.CI.update(can_list) CS_SP = convert_to_capnp(CS_SP) populate_ford_pscm_status(self.CP, self.CI.can_parsers, CS_SP, CS.canValid) + if self.ford_joint_control is not None and self.ford_joint_control.fault: + # Surface a latched loss of command history through the normal steering + # fault alert; never silently leave the UI engaged with this trial disabled. + CS.steerFaultTemporary = True # Update radar tracks from CAN RD: structs.RadarDataT | None = self.RI.update(can_list) @@ -269,7 +275,7 @@ class Car: cs_sp_send.carStateSP = CS_SP self.pm.send('carStateSP', cs_sp_send) - def controls_update(self, CS: car.CarState, CC: car.CarControl, CC_SP: custom.CarControlSP): + def controls_update(self, CS: car.CarState, CC: car.CarControl, CC_SP: custom.CarControlSP, pscm_status=None): """control update loop, driven by carControl""" if not self.initialized_prev: @@ -282,8 +288,16 @@ class Car: if self.sm.all_alive(['carControl']): # send car controls over can now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) - self.last_actuators_output, can_sends = self.CI.apply(CC, convert_carControlSP(CC_SP), now_nanos) + control_sp = convert_carControlSP(CC_SP) + if self.ford_joint_control is not None: + CC = self.ford_joint_control.prepare(CC, control_sp, CS, now_nanos * 1e-9, + fresh=self.sm.all_checks(['carControl', 'carControlSP']), pscm_status=pscm_status) + self.last_actuators_output, can_sends = self.CI.apply(CC, control_sp, now_nanos) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) + if self.ford_joint_control is not None: + self.ford_joint_control.record_sent(can_sends, now_nanos) + if self.sm.frame % 20 == 0: + cloudlog.event('Ford joint path tracking', **self.ford_joint_control.diagnostics) self.CC_prev = CC @@ -295,7 +309,7 @@ class Car: initialized = (not any(e.name == EventName.selfdriveInitializing for e in self.sm['onroadEvents']) and self.sm.seen['onroadEvents']) if not self.CP.passive and initialized: - self.controls_update(CS, self.sm['carControl'], self.sm['carControlSP']) + self.controls_update(CS, self.sm['carControl'], self.sm['carControlSP'], CS_SP.fordPscmStatus) self.initialized_prev = initialized self.CS_prev = CS diff --git a/openpilot/selfdrive/car/ford_joint_control.py b/openpilot/selfdrive/car/ford_joint_control.py new file mode 100644 index 0000000000..62324bfad6 --- /dev/null +++ b/openpilot/selfdrive/car/ford_joint_control.py @@ -0,0 +1,148 @@ +"""Default-off joint C0/C1 trial at the existing Ford transmit boundary. + +The observer consumes decoded packets queued to panda, not tentative requests. +It estimates older-firmware state; it cannot observe ECU RAM or prove acceptance. +""" + +import math + +from opendbc.car.ford.values import FordFlags + + +def joint_control_enabled(CP, params): + return bool( + CP.brand == 'ford' + and CP.flags & FordFlags.CANFD + and params.get_bool('FordModelActionController') + and params.get_bool('FordPscmJointControl') + and not params.get_bool('FordGeometryReference') + and not params.get_bool('JoystickDebugMode') + ) + + +def select_joint_control(CP, params): + # No calibration or native-library load on the normal/default path. + return FordJointControl(CP) if joint_control_enabled(CP, params) else None + + +class FordJointControl: + def __init__(self, CP): + from opendbc.can import CANParser + from opendbc.car.ford.fordcan import CanBus + from openpilot.selfdrive.controls.lib.ford_joint.model import MainRequest + from openpilot.selfdrive.controls.lib.ford_joint.angle import AngleModel + from openpilot.selfdrive.controls.lib.ford_joint.encoder import PairedRelease + + self.wheelbase, self.ratio = CP.wheelbase, CP.steerRatio + if not all(math.isfinite(v) and v > 0 for v in (self.wheelbase, self.ratio)): + raise ValueError('Joint control requires finite positive vehicle geometry') + self.request = MainRequest(native_lookup=True) + self.angle = AngleModel(self.request.cal) + self.encoder = PairedRelease(self.request, preserve_now=True) + self.bus = CanBus(CP).main + self.parser = CANParser('ford_lincoln_base_pt', [('LateralMotionControl2', 100)], self.bus) + self.last_time = None + self.last_sent_time = None + self.phase = 0.0 + self.sent = (0.0, 0.0, False) + self.measurement = (0.0, 0.0, 0.0) + self.fault = '' + self.diagnostics = {'hypothesis': 'ford-joint-v24', 'status': 'inactive'} + + def advance(self, now): + """Retain channel/filter state through inactive commands and driver override.""" + if self.last_time is not None: + elapsed = now - self.last_time + if not 0.0 <= elapsed <= 0.1: + # Lost command history cannot be repaired by pretending the held state + # reset. Disable this trial until the next onroad process start. + self.fault = 'command_timing_gap' + else: + self.phase += elapsed + ticks = int((self.phase + 1e-12) / 0.008) + self.phase -= ticks * 0.008 + speed, angle, yaw = self.measurement + for _ in range(ticks): + r = self.request.step(speed, self.sent[0], self.sent[1], active=self.sent[2], freeze_i=True) + self.angle.step(speed, r['filtered_curvature'], angle, yaw, yaw * speed / 3.6, self.wheelbase, self.ratio) + self.last_time = now + + def prepare(self, CC, CC_SP, CS, now, *, fresh=True, pscm_status=None): + from openpilot.selfdrive.controls.lib.ford_joint.inverse import invert_angle + + self.advance(now) + path = CC_SP.fordLateralPath + target = float(CC.actuators.steeringAngleDeg) + finite = all(math.isfinite(v) for v in (CS.vEgo, CS.steeringAngleDeg, CS.yawRate, target)) + if finite: + # The route extractor negated carState.yawRate; the audited inverse negated + # it back. Native carState yaw and wheel angle already share our CAN sign. + self.measurement = (max(0.0, CS.vEgo * 3.6), CS.steeringAngleDeg, CS.yawRate) + status_fresh = pscm_status is not None and pscm_status.valid and pscm_status.canMonoTime > 0 and -0.005 <= now - pscm_status.canMonoTime * 1e-9 <= 0.15 + override = bool(CS.steeringPressed or (status_fresh and (pscm_status.limit == 3 or pscm_status.denied))) + valid = bool( + fresh + and CS.canValid + and finite + and 0.3 <= CS.vEgo <= 55 + and abs(CS.yawRate) <= 3 + and path.enabled + and path.valid + and not CS.steerFaultTemporary + and not CS.steerFaultPermanent + ) + # A missing previous transmit receipt means the prediction is not synchronized. + if self.last_sent_time is not None and now - self.last_sent_time > 0.1: + self.fault = 'missing_transmit_history' + active = bool(CC.latActive and valid and not override and not self.fault) + command = (0.0, 0.0) + details = {} + if active: + try: + speed, angle, yaw = self.measurement + inverse = invert_angle(self.angle, speed, target, angle, yaw, yaw * speed / 3.6, self.wheelbase, self.ratio) + # Firmware phase is estimated from elapsed time. Cover the 1/2 firmware + # ticks before the next nominal 100 Hz transmit; all phases are tested. + ticks = max(1, min(2, int((self.phase + 0.01 + 1e-12) / 0.008))) + command, info = self.encoder.choose(speed, inverse['curvature'], phase=0 if ticks == 2 else 2) + details = { + 'requested_angle': target, + 'reachable_angle': inverse['reachable_target'], + 'target_curvature': inverse['curvature'], + 'predicted_curvature': float(info['first_state'][3]), + 'accel_limited': inverse['accel_limited'], + } + except (ValueError, OverflowError, ArithmeticError): + self.fault = 'invalid_prediction' + active = False + command = (0.0, 0.0) + # The inverse operates in CAN/pinion coordinates. CarController negates + # FordLateralPath on TX, so negate here exactly once to retain the tested sign. + path.pathOffset, path.pathAngle = -float(command[0]), -float(command[1]) + path.curvature = path.curvatureRate = 0.0 + path.valid = active + cc = CC.as_reader().as_builder() if hasattr(CC, 'as_reader') else CC.as_builder() + cc.latActive = active + self.diagnostics = { + 'hypothesis': 'ford-joint-v24', + 'status': self.fault or ('active' if active else 'inactive'), + 'driver_override': override, + 'held_c0': self.request.c0, + 'held_c1': self.request.c1, + 'filtered_curvature': self.request.filtered, + 'wire_command': tuple(map(float, command)), + **details, + } + return cc + + def record_sent(self, can_sends, now_nanos): + packets = [msg for msg in can_sends if msg[0] == 0x3D6 and msg[2] == self.bus] + if not packets: + return + self.parser.update([now_nanos, packets]) + values = self.parser.vl['LateralMotionControl2'] + if values['LatCtlCurv_No_Actl'] != 0.0 or values['LatCtlCrv_NoRate2_Actl'] != 0.0: + self.fault = 'unexpected_curvature_channel' + self.sent = (values['LatCtlPathOffst_L_Actl'], values['LatCtlPath_An_Actl'], values['LatCtl_D2_Rq'] == 2) + self.last_sent_time = now_nanos * 1e-9 + self.diagnostics['wire_sent'] = self.sent diff --git a/openpilot/selfdrive/car/tests/test_ford_joint_control.py b/openpilot/selfdrive/car/tests/test_ford_joint_control.py new file mode 100644 index 0000000000..3224243897 --- /dev/null +++ b/openpilot/selfdrive/car/tests/test_ford_joint_control.py @@ -0,0 +1,253 @@ +"""Exercise the opt-in trial through the production Ford CAN packer.""" +import copy +import itertools +import json +import math +from collections import defaultdict +from types import SimpleNamespace + +import numpy as np +import pytest + +from opendbc.car import Bus, structs +from opendbc.car.ford.carcontroller import CarController +from opendbc.car.ford.values import FordFlags +from openpilot.common.params import Params, ParamKeyFlag +from openpilot.selfdrive.car.ford_joint_control import FordJointControl, joint_control_enabled, select_joint_control +from openpilot.selfdrive.controls.lib.ford_joint.angle import AngleModel +from openpilot.selfdrive.controls.lib.ford_joint.encoder import PairedRelease, state +from openpilot.selfdrive.controls.lib.ford_joint.inverse import invert_angle +from openpilot.selfdrive.controls.lib.ford_joint.model import MainRequest +from openpilot.selfdrive.controls.tests.test_ford_model_action_adapter import update +from openpilot.selfdrive.controls.tests.test_ford_model_action_selection import car_params, startup + + +def cp(): + return structs.CarParams(brand='ford', flags=int(FordFlags.CANFD), carFingerprint='FORD_F_150_LIGHTNING_MK1', wheelbase=3.7, steerRatio=16.9) + + +def controls(target=30., active=True): + cc = structs.CarControl(latActive=active, longActive=True) + cc.actuators.steeringAngleDeg = target + cc.actuators.accel = .3 + sp = structs.CarControlSP() + sp.fordLateralPath.enabled = sp.fordLateralPath.valid = True + return cc, sp + + +class Pipeline: + def __init__(self): + self.joint = FordJointControl(cp()) + self.sender = CarController({Bus.pt: 'ford_lincoln_base_pt'}, cp(), structs.CarParamsSP()) + self.cs = structs.CarState(vEgo=5.36, vEgoRaw=5.36, canValid=True) + self.vehicle = SimpleNamespace(out=self.cs, acc_tja_status_stock_values=defaultdict(int), + lkas_status_stock_values=defaultdict(int), buttons_stock_values=defaultdict(int)) + + def tick(self, now, target=30., active=True, **kwargs): + cc, sp = controls(target, active) + result = self.joint.prepare(cc.as_reader(), sp, self.cs, now, **kwargs) + _, packets = self.sender.update(result.as_reader(), sp, self.vehicle, round(now * 1e9)) + assert sum(p[0] == 0x3d6 for p in packets) == 1 + self.joint.record_sent(packets, round(now * 1e9)) + assert result.longActive and result.actuators.accel == cc.actuators.accel + wire = self.joint.parser.vl['LateralMotionControl2'] + assert wire['LatCtlCurv_No_Actl'] == wire['LatCtlCrv_NoRate2_Actl'] == 0. + assert abs(wire['LatCtlPathOffst_L_Actl']) <= 5.11 + 1e-10 + assert abs(wire['LatCtlPath_An_Actl']) <= .5 + 1e-10 + assert wire['LatCtl_D2_Rq'] == (2 if result.latActive else 0) + assert self.joint.sent == pytest.approx((*self.joint.diagnostics['wire_command'], result.latActive)) + return result + + +@pytest.mark.parametrize('master,trial,geometry,joystick', itertools.product((False, True), repeat=4)) +@pytest.mark.parametrize('compatible', [False, True]) +def test_startup_gate_matches_both_processes(master, trial, geometry, joystick, compatible): + params = SimpleNamespace(get_bool=lambda k: {'FordModelActionController': master, 'FordPscmJointControl': trial, + 'FordGeometryReference': geometry, 'JoystickDebugMode': joystick}.get(k, False)) + params_cp = car_params(flags=FordFlags.CANFD if compatible else 0) + expected = compatible and master and trial and not geometry and not joystick + assert joint_control_enabled(params_cp, params) == expected + selected = startup(params_cp, params).ford_path_controller + assert bool(selected and selected.joint_control) == expected + if not expected: + assert select_joint_control(params_cp, params) is None + if not master: + assert selected is None + + +def test_parameter_is_persistent_default_off(tmp_path): + params = Params(str(tmp_path)) + assert params.get_default_value('FordPscmJointControl') is False + assert not params.get_bool('FordPscmJointControl') + for flag in (ParamKeyFlag.PERSISTENT, ParamKeyFlag.BACKUP): + assert b'FordPscmJointControl' in params.all_keys(flag) + + +def test_reference_carrier_adds_no_second_feedback_loop(): + from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController + c = FordModelActionController(joint_control=True, c0_time_based=True) + for i in range(100): + p = update(c, now=1+i*.01, current_curvature=-.005) + assert p.valid and p.path_offset == p.path_angle == p.curvature == p.curvature_rate == 0. + assert c.core.correction == c.core.proportional == c.core.offset_proportional == 0. + assert not c.set_c0_time_based(True, lateral_engaged=False) + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_can_coordinates_entry_reversal_release_and_100hz(sign): + p = Pipeline() + for i in range(250): + target = sign * (30. if i < 80 else -30. if i < 160 else 0.) + assert p.tick(1+i*.01, target).latActive + if i == 0: + # The existing sender negates OP path fields; the adapter compensates once. + assert sign * p.joint.sent[0] > 0 and sign * p.joint.sent[1] > 0 + json.dumps(p.joint.diagnostics, allow_nan=False) + assert not p.joint.fault + + +def test_observer_follows_packed_command_only_and_inactive_slew(): + p = Pipeline() + cc, sp = controls() + before = state(p.joint.request).copy() + p.joint.prepare(cc, sp, p.cs, 1.) # An untransmitted candidate is not held input. + p.joint.advance(1.01) + np.testing.assert_array_equal(state(p.joint.request), before) + for i in range(100): + p.tick(1.02+i*.01, 60.) + held = copy.copy(p.joint.request) + p.cs.steeringPressed = True + result = p.tick(2.02, 60.) + assert not result.latActive and p.joint.sent == (0., 0., False) + assert abs(p.joint.request.c0) > 0 or abs(p.joint.request.c1) > 0 + held = copy.copy(p.joint.request) + phase = p.joint.phase + p.tick(2.03, 60.) + ticks = int((phase+.01+1e-12)/.008) + for _ in range(ticks): + held.step(5.36*3.6, 0., 0., active=False, freeze_i=True) + np.testing.assert_allclose(state(p.joint.request), state(held), atol=1e-12) + p.cs.steeringPressed = False + assert p.tick(2.04, -30.).latActive + assert p.joint.diagnostics['requested_angle'] == -30. + + +@pytest.mark.parametrize('limit,denied,active', [(0, False, True), (2, False, True), (3, False, False), (0, True, False)]) +def test_limit_reached_does_not_freeze_but_override_inhibits(limit, denied, active): + p = Pipeline() + status = SimpleNamespace(valid=True, canMonoTime=1_000_000_000, limit=limit, denied=denied) + assert p.tick(1., pscm_status=status).latActive == active + + +@pytest.mark.parametrize('field,value', [('vEgo', 0.), ('vEgo', .29), ('vEgo', 56.), ('yawRate', 3.1), + ('steeringAngleDeg', math.nan), ('vEgo', math.inf), ('canValid', False), + ('steerFaultTemporary', True), ('steerFaultPermanent', True)]) +def test_bad_measurements_never_send_previous_request(field, value): + p = Pipeline() + p.tick(1.) + setattr(p.cs, field, value) + assert not p.tick(1.01).latActive + assert p.joint.sent == (0., 0., False) + json.dumps(p.joint.diagnostics, allow_nan=False) + + +@pytest.mark.parametrize('target', [math.nan, math.inf, -math.inf]) +def test_nonfinite_target_and_stale_control_are_inactive(target): + p = Pipeline() + assert not p.tick(1., target).latActive + assert not p.tick(1.01, fresh=False).latActive + assert p.tick(1.02).latActive + + +@pytest.mark.parametrize('next_time', [.9, 1.101]) +def test_unknown_clock_history_latches_inactive(next_time): + p = Pipeline() + p.tick(1.) + assert not p.tick(next_time).latActive + assert not p.tick(next_time+.01).latActive + assert p.joint.fault + + +def test_angle_inverse_matches_forward_stage_across_speeds(): + rng = np.random.default_rng(418) + for _ in range(80): + m = AngleModel(MainRequest().cal) + m.yaw_acc, m.bank_residual, m.angle_bias = rng.uniform(-1., 1., 3) + speed = float(rng.uniform(1.1, 180.)) + target, angle = map(float, rng.uniform(-300., 300., 2)) + yaw = float(rng.uniform(-.5, .5)) + result = invert_angle(m, speed, target, angle, yaw, yaw*speed/3.6, 3.7, 16.9) + actual = m.step(speed, result['curvature'], angle, yaw, yaw*speed/3.6, 3.7, 16.9) + assert actual == pytest.approx(result['reachable_target'], abs=1e-9) + assert abs(result['bounded_accel']) <= result['allowance'] + + +def test_candidate_does_not_mutate_state_and_native_step_matches_python(): + rng = np.random.default_rng(919) + for j in range(40): + m = MainRequest(native_lookup=True) + m.c0, m.c1, m.filtered = map(float, rng.uniform([-5., -.49, -.08], [5., .49, .08])) + m.fast = bool(j % 2) + speed, curvature = float(rng.uniform(1.1, 180.)), float(rng.uniform(-.1, .1)) + old = state(m).copy() + pair, info = PairedRelease(m).choose(speed, curvature, phase=j % 10) + np.testing.assert_array_equal(state(m), old) + for _ in range(info['count']): + m.step(speed, *pair, freeze_i=True) + np.testing.assert_allclose(state(m), info['first_state'], atol=1e-12, rtol=0) + assert abs(info['first_state'][3]-info['planned_target']) <= info['immediate_error_bound']+1e-12 + + +def test_card_transmit_hook_and_fault_alert_use_actual_source(): + import ast + from pathlib import Path + from openpilot.cereal import custom + from openpilot.selfdrive.car.helpers import convert_carControlSP + + source_path = Path(__file__).resolve().parents[1] / 'card.py' + tree = ast.parse(source_path.read_text()) + car = next(n for n in tree.body if isinstance(n, ast.ClassDef) and n.name == 'Car') + method = next(n for n in car.body if isinstance(n, ast.FunctionDef) and n.name == 'controls_update') + branch = next(n for n in method.body if isinstance(n, ast.If) and ast.unparse(n.test) == "self.sm.all_alive(['carControl'])") + code = compile(ast.Module(body=[branch], type_ignores=[]), str(source_path), 'exec') + p = Pipeline() + sent = [] + owner = SimpleNamespace(ford_joint_control=p.joint, sm=SimpleNamespace(all_alive=lambda _: True, all_checks=lambda _: True, frame=1), + CI=SimpleNamespace(apply=lambda cc, sp, now: p.sender.update(cc.as_reader(), sp, p.vehicle, now)), + pm=SimpleNamespace(send=lambda service, data: sent.append((service, data)))) + cc, _ = controls() + sp = custom.CarControlSP.new_message() + sp.fordLateralPath.enabled = sp.fordLateralPath.valid = True + exec(code, {'self': owner, 'CC': cc.as_reader(), 'CC_SP': sp, 'CS': p.cs, 'pscm_status': None, 'REPLAY': False, + 'time': SimpleNamespace(monotonic=lambda: 1.), 'convert_carControlSP': convert_carControlSP, + 'can_list_to_can_capnp': lambda data, **kwargs: data}) + assert sent[0][0] == 'sendcan' and p.joint.sent[2] + assert p.joint.last_sent_time == 1. + assert owner.CC_prev.latActive + + update_method = next(n for n in car.body if isinstance(n, ast.FunctionDef) and n.name == 'state_update') + alert = next(n for n in update_method.body if isinstance(n, ast.If) and 'self.ford_joint_control.fault' in ast.unparse(n.test)) + alert_code = compile(ast.Module(body=[alert], type_ignores=[]), str(source_path), 'exec') + p.joint.fault = 'command_timing_gap' + exec(alert_code, {'self': owner, 'CS': p.cs}) + assert p.cs.steerFaultTemporary + + +def test_nonfinite_native_result_latches_inactive(monkeypatch): + from openpilot.selfdrive.controls.lib.ford_joint.encoder import LIB + p = Pipeline() + monkeypatch.setattr(LIB, 'paired_select', lambda *args: None) + assert not p.tick(1.).latActive + assert p.joint.fault == 'invalid_prediction' + + +def test_duplicate_clock_and_missing_receipts_cannot_advance_unknown_history(): + p = Pipeline() + p.tick(1.) + assert p.tick(1.).latActive + assert state(p.joint.request).tolist() == [0., 0., 0., 0., 0.] + # Prepare can run without sending, but stale TX history must end the trial. + for now in (1.05, 1.10, 1.15): + cc, sp = controls() + result = p.joint.prepare(cc, sp, p.cs, now) + assert not result.latActive and p.joint.fault == 'missing_transmit_history' diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index 5c3eeec42c..8225544570 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -57,7 +57,9 @@ class Controls(ControlsExt): self.desired_curvature = 0.0 self.ford_path_controller = select_model_action_controller(self.CP, self.params.get_bool("FordModelActionController"), c0_time_based=self.params.get_bool("FordC0TimeBased"), - direct_path=self.params.get_bool("FordGeometryReference")) + direct_path=self.params.get_bool("FordGeometryReference"), + joint_control=self.params.get_bool("FordPscmJointControl") and + not self.params.get_bool("JoystickDebugMode")) 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_joint/SConscript b/openpilot/selfdrive/controls/lib/ford_joint/SConscript new file mode 100644 index 0000000000..12a06b24a9 --- /dev/null +++ b/openpilot/selfdrive/controls/lib/ford_joint/SConscript @@ -0,0 +1,3 @@ +Import('env') + +env.SharedLibrary('encoder', 'encoder.cc', LIBS=[]) diff --git a/openpilot/selfdrive/controls/lib/ford_joint/__init__.py b/openpilot/selfdrive/controls/lib/ford_joint/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/selfdrive/controls/lib/ford_joint/angle.py b/openpilot/selfdrive/controls/lib/ford_joint/angle.py new file mode 100644 index 0000000000..e2a6bae8e6 --- /dev/null +++ b/openpilot/selfdrive/controls/lib/ford_joint/angle.py @@ -0,0 +1,31 @@ +"""Measurement states of the recovered curvature-to-angle stage, before torque.""" + +import math + +from openpilot.selfdrive.controls.lib.ford_joint.model import bracket, interp_int, clip + + +class AngleModel: + def __init__(self, cal): + self.cal = cal + self.speed_bp = cal.read(0xFEF25E40, '8H') + self.tables = {address: cal.read(address, '8H') for address in (0xFEF25CA0, 0xFEF25CB0)} + self.yaw_acc = self.bank_residual = self.angle_bias = 0.0 + + def step(self, speed_kmh, curvature, angle, yaw, accel, wheelbase, ratio): + c = self.cal + si, sf = bracket(int(speed_kmh * 256) & 0xFFFF, self.speed_bp) + v = speed_kmh / 3.6 + self.yaw_acc += c.h(0xFEF25A90) / 65536 * (v * yaw - self.yaw_acc) + self.bank_residual += c.h(0xFEF25A88) / 65536 * (accel - v * yaw - self.bank_residual) + opposite = int(self.yaw_acc > 0) - int(self.yaw_acc < 0) != int(self.bank_residual > 0) - int(self.bank_residual < 0) + allowance = 3.0 + (abs(self.bank_residual) if opposite else 0.0) + requested_accel = clip(curvature * v * v, -allowance, allowance) + control_accel = requested_accel * c.b(0xFEF25AB1) / 128 + interp_int(self.tables[0xFEF25CA0], si, sf) / 2048 * (requested_accel - self.yaw_acc) + understeer = c.read(0xFEF25A9A, 'h')[0] / 16384 + floor_v = max(v, c.b(0xFEF25AB7) / 32, 1e-6) + conversion = 180 / math.pi * ratio + target = ((self.bank_residual * c.read(0xFEF25AB0, 'b')[0] / 64 + control_accel) * understeer + control_accel / floor_v**2 * wheelbase) * conversion + error = (floor_v * understeer * yaw + yaw / floor_v * wheelbase) * conversion - angle - self.angle_bias + self.angle_bias += interp_int(self.tables[0xFEF25CB0], si, sf) / 65536 * error + return target + self.angle_bias * c.read(0xFEF25AB2, 'b')[0] / 64 diff --git a/openpilot/selfdrive/controls/lib/ford_joint/calibration.json b/openpilot/selfdrive/controls/lib/ford_joint/calibration.json new file mode 100644 index 0000000000..bd29c0212e --- /dev/null +++ b/openpilot/selfdrive/controls/lib/ford_joint/calibration.json @@ -0,0 +1,475 @@ +{ + "firmware": "ML3V-14D003-BD", + "calibration": "ML34-14D007-EDL", + "firmware_sha256": "8de3eb1f8191b13b57de07430c33f7b723f11b6ed2144991f91dc56476bbbf62", + "note": "Recovered numerical calibration only; RL38 equivalence unverified. No executable firmware.", + "entries": { + "0xfef25e40": { + "format": "8H", + "values": [ + 0, + 3840, + 7680, + 15360, + 25600, + 33280, + 40960, + 56320 + ] + }, + "0xfef25ca0": { + "format": "8H", + "values": [ + 0, + 410, + 410, + 410, + 614, + 1024, + 410, + 410 + ] + }, + "0xfef25cb0": { + "format": "8H", + "values": [ + 1311, + 1311, + 1147, + 983, + 819, + 590, + 393, + 229 + ] + }, + "0xfef25d98": { + "format": "10f", + "values": [ + 10.0, + 20.0, + 30.0, + 40.0, + 50.0, + 60.0, + 70.0, + 80.0, + 100.0, + 150.0 + ] + }, + "0xfef258fc": { + "format": "8B", + "values": [ + 3, + 3, + 4, + 5, + 7, + 10, + 11, + 11 + ] + }, + "0xfef25b78": { + "format": "8f", + "values": [ + 0.0, + 0.019999999552965164, + 0.05000000074505806, + 0.10000000149011612, + 0.11749999970197678, + 0.125, + 0.125, + 0.125 + ] + }, + "0xfef25b98": { + "format": "8f", + "values": [ + 15.0, + 15.0, + 25.0, + 41.666664123535156, + 55.55555725097656, + 72.22222137451172, + 88.88888549804688, + 122.22222137451172 + ] + }, + "0xfef25bb8": { + "format": "8f", + "values": [ + 15.0, + 15.0, + 25.0, + 41.666664123535156, + 50.0, + 46.94444274902344, + 57.77777862548828, + 79.44444274902344 + ] + }, + "0xfef25be8": { + "format": "8f", + "values": [ + 0.0, + 0.0, + 5.0, + 5.0, + 18.518518447875977, + 24.074073791503906, + 29.629629135131836, + 29.0 + ] + }, + "0xfef25c60": { + "format": "8H", + "values": [ + 512, + 512, + 512, + 1024, + 1536, + 1536, + 1536, + 1536 + ] + }, + "0xfef25d70": { + "format": "10f", + "values": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + }, + "0xfef25dc0": { + "format": "10f", + "values": [ + 0.19599999487400055, + 0.09799999743700027, + 0.06530000269412994, + 0.04899999871850014, + 0.03920000046491623, + 0.03269999846816063, + 0.02800000086426735, + 0.02449999935925007, + 0.019600000232458115, + 0.013100000098347664 + ] + }, + "0xfef25de8": { + "format": "10f", + "values": [ + 0.019999999552965164, + 0.004999999888241291, + 0.002222222276031971, + 0.0012499999720603228, + 0.0007999999797903001, + 0.0005555555690079927, + 0.0004081632650922984, + 0.0003124999930150807, + 0.00019999999494757503, + 8.888888987712562e-05 + ] + }, + "0xfef25e20": { + "format": "8H", + "values": [ + 24576, + 24576, + 24576, + 24576, + 24576, + 24576, + 16384, + 819 + ] + }, + "0xfef25e30": { + "format": "8H", + "values": [ + 24576, + 24576, + 24576, + 24576, + 20480, + 18022, + 16384, + 819 + ] + }, + "0xfef25a58": { + "format": "f", + "values": [ + 2.0 + ] + }, + "0xfef259ec": { + "format": "f", + "values": [ + 1.0 + ] + }, + "0xfef259f0": { + "format": "f", + "values": [ + 2.0 + ] + }, + "0xfef259f4": { + "format": "f", + "values": [ + 0.6000000238418579 + ] + }, + "0xfef25a14": { + "format": "f", + "values": [ + 30.0 + ] + }, + "0xfef25a04": { + "format": "f", + "values": [ + 300.0 + ] + }, + "0xfef2596c": { + "format": "f", + "values": [ + 1200.0 + ] + }, + "0xfef259a0": { + "format": "f", + "values": [ + 0.20000000298023224 + ] + }, + "0xfef2599c": { + "format": "f", + "values": [ + 1.350000023841858 + ] + }, + "0xfef259a4": { + "format": "f", + "values": [ + -10.0 + ] + }, + "0xfef259a8": { + "format": "f", + "values": [ + 0.15000000596046448 + ] + }, + "0xfef25a70": { + "format": "H", + "values": [ + 1229 + ] + }, + "0xfef25a72": { + "format": "H", + "values": [ + 0 + ] + }, + "0xfef25a60": { + "format": "H", + "values": [ + 256 + ] + }, + "0xfef25aa5": { + "format": "B", + "values": [ + 0 + ] + }, + "0xfef25aa6": { + "format": "B", + "values": [ + 38 + ] + }, + "0xfef25aa1": { + "format": "B", + "values": [ + 0 + ] + }, + "0xfef25aa2": { + "format": "B", + "values": [ + 160 + ] + }, + "0xfef25a9e": { + "format": "B", + "values": [ + 0 + ] + }, + "0xfef25a9f": { + "format": "B", + "values": [ + 32 + ] + }, + "0xfef25aa4": { + "format": "B", + "values": [ + 255 + ] + }, + "0xfef25aa7": { + "format": "B", + "values": [ + 26 + ] + }, + "0xfef25aa3": { + "format": "B", + "values": [ + 9 + ] + }, + "0xfef25aa0": { + "format": "B", + "values": [ + 38 + ] + }, + "0xfef25a74": { + "format": "H", + "values": [ + 51 + ] + }, + "0xfef25a00": { + "format": "f", + "values": [ + 0.30000001192092896 + ] + }, + "0xfef25a10": { + "format": "f", + "values": [ + 0.014999999664723873 + ] + }, + "0xfef259e8": { + "format": "f", + "values": [ + 0.0003000000142492354 + ] + }, + "0xfef259e4": { + "format": "f", + "values": [ + 0.003000000026077032 + ] + }, + "0xfef25a0c": { + "format": "f", + "values": [ + 0.30000001192092896 + ] + }, + "0xfef259fc": { + "format": "f", + "values": [ + 2.799999952316284 + ] + }, + "0xfef25968": { + "format": "f", + "values": [ + -0.30000001192092896 + ] + }, + "0xfef25960": { + "format": "f", + "values": [ + 0.30000001192092896 + ] + }, + "0xfef25964": { + "format": "f", + "values": [ + -0.4000000059604645 + ] + }, + "0xfef2595c": { + "format": "f", + "values": [ + 0.4000000059604645 + ] + }, + "0xfef25a08": { + "format": "f", + "values": [ + 0.10000000149011612 + ] + }, + "0xfef259f8": { + "format": "f", + "values": [ + 1.5 + ] + }, + "0xfef25a90": { + "format": "H", + "values": [ + 6554 + ] + }, + "0xfef25a88": { + "format": "H", + "values": [ + 1627 + ] + }, + "0xfef25ab7": { + "format": "B", + "values": [ + 32 + ] + }, + "0xfef25a9a": { + "format": "h", + "values": [ + 33 + ] + }, + "0xfef25ab1": { + "format": "B", + "values": [ + 128 + ] + }, + "0xfef25ab0": { + "format": "b", + "values": [ + 0 + ] + }, + "0xfef25ab2": { + "format": "b", + "values": [ + -64 + ] + } + } +} diff --git a/openpilot/selfdrive/controls/lib/ford_joint/encoder.cc b/openpilot/selfdrive/controls/lib/ford_joint/encoder.cc new file mode 100644 index 0000000000..c36ec122c4 --- /dev/null +++ b/openpilot/selfdrive/controls/lib/ford_joint/encoder.cc @@ -0,0 +1,100 @@ +// Opt-in Ford joint encoder. Numerical request state is estimated, not ECU RAM. +#include +#include +#include + +static double clip(double x, double lo, double hi) { + return std::min(std::max(x, lo), hi); +} + +// State: held C0, held C1, internal I, filtered curvature, fast-slew latch. +// Parameters: g0/g1, base alpha, heading low/high/gain, alpha max, speed, +// interaction threshold/value, C0 slow/fast, C1 slow/fast, latch clear C0/C1, +// opposite C0/I thresholds, I gain, delta low/high, I low/high, release rate. +static void step(double *s, const double *p, double c0, double c1) { + if (p[9] < p[8]) { + s[4] = 1; + } else if (std::abs(c0) <= p[14] && std::abs(c1) <= p[15]) { + s[4] = 0; + } + double r0 = p[s[4] ? 11 : 10], r1 = p[s[4] ? 13 : 12]; + s[0] += clip(c0 - s[0], -r0 * .008, r0 * .008); + s[1] += clip(c1 - s[1], -r1 * .008, r1 * .008); + if (std::abs(s[0]) > p[16] && std::abs(s[2]) > p[17] && s[0] * s[2] < 0) { + s[2] += clip(-s[2], -p[23], p[23]); + } else { + s[2] = clip(s[2] + clip(s[0], p[19], p[20]) * p[18], p[21], p[22]); + } + double raw = p[0] * (s[0] + s[2]) + p[1] * s[1]; + double w = clip(std::abs(s[1] * p[7]) - p[3], 0, std::max(1e-6, p[4] - p[3])) / std::max(1e-6, p[4] - p[3]); + double alpha = std::min(p[6], p[2] + p[5] * w); + s[3] += alpha * (raw - s[3]); +} + +extern "C" double paired_cost(const double *initial, const double *p, double target, + const double *pref, int count, double c0, double c1, double *first) { + double s[5]; + std::memcpy(s, initial, sizeof(s)); + double steady_raw = p[0] * pref[0] + p[1] * pref[1]; + double steady_error = (steady_raw - target) / .01, cost = 0; + auto tick = [&](double a, double b) { + step(s, p, a, b); + double error = (s[3] - target) / .01; + cost += .008 * (error * error - steady_error * steady_error); + }; + for (int j = 0; j < count; j++) tick(c0, c1); + if (first) std::memcpy(first, s, sizeof(s)); + // Include both channels' entire return, followed by the exact filter tail. + // There is no adjustable planning horizon or retained future command plan. + int n = 2 + std::ceil(std::max(std::abs(s[0] - pref[0]) / std::min(p[10], p[11]), + std::abs(s[1] - pref[1]) / std::min(p[12], p[13])) / .008); + for (int j = 0; j < n; j++) tick(pref[0], pref[1]); + double w = clip(std::abs(s[1] * p[7]) - p[3], 0, p[4] - p[3]) / (p[4] - p[3]); + double alpha = std::min(p[6], p[2] + p[5] * w), a = 1 - alpha; + double d = (s[3] - steady_raw) / .01; + return cost + .008 * (2 * steady_error * d * a / alpha + d * d * a * a / (1 - a * a)); +} + +extern "C" void paired_select(const double *initial, const double *p, double target, + const double *pref, int count, const double *c0s, int n0, + const double *c1s, int n1, int preserve_now, double *result) { + double best = 1e300, best_move = 1e300, best_remaining = 1e300; + double first[5]; + double max_error = 1e300, anchor_score = 1e300, anchor_move = 1e300, anchor_remaining = 1e300; + if (preserve_now) { + // Preserve the C1-anchored policy's immediate target accuracy. + // An inequality against a feasible reference, not a new gain/deadband. + for (int i = 0; i < n0; i++) { + double cost = paired_cost(initial, p, target, pref, count, c0s[i], pref[1], first); + double score = std::nearbyint(cost * 1e12) / 1e12; + double move = std::abs(c0s[i] - initial[0]), remaining = std::abs(c0s[i] - pref[0]); + if (score < anchor_score || (score == anchor_score && (move < anchor_move || (move == anchor_move && remaining < anchor_remaining)))) { + anchor_score = score; + anchor_move = move; + anchor_remaining = remaining; + max_error = std::abs(first[3] - target); + } + } + } + for (int i = 0; i < n0; i++) { + for (int j = 0; j < n1; j++) { + double cost = paired_cost(initial, p, target, pref, count, c0s[i], c1s[j], first); + if (std::abs(first[3] - target) > max_error + 1e-12) continue; + // Numerical equality only. Tie breaks cannot trade worse tracking for + // less channel motion; normalize them using the existing field spans. + double score = std::nearbyint(cost * 1e12) / 1e12; + double move = std::abs(c0s[i] - initial[0]) / 5.11 + std::abs(c1s[j] - initial[1]) / .5; + double remaining = std::abs(c0s[i] - pref[0]) / 5.11 + std::abs(c1s[j] - pref[1]) / .5; + if (score < best || (score == best && (move < best_move || (move == best_move && remaining < best_remaining)))) { + best = score; + best_move = move; + best_remaining = remaining; + result[0] = c0s[i]; + result[1] = c1s[j]; + result[2] = cost; + std::memcpy(result + 3, first, 5 * sizeof(double)); + } + } + } + result[8] = max_error; +} diff --git a/openpilot/selfdrive/controls/lib/ford_joint/encoder.py b/openpilot/selfdrive/controls/lib/ford_joint/encoder.py new file mode 100644 index 0000000000..43093a92e6 --- /dev/null +++ b/openpilot/selfdrive/controls/lib/ford_joint/encoder.py @@ -0,0 +1,112 @@ +"""Joint C0/C1 allocation: one update, then a fixed steady-return policy. + +No future tape, retained trajectory, maneuver modes or tunable tracking gain. +The next-update accuracy constraint is enabled for the trial. +""" + +import copy +import ctypes +import sys +from pathlib import Path +import numpy as np + +from openpilot.selfdrive.controls.lib.ford_joint.model import DT, interp, interp_int +from openpilot.selfdrive.controls.lib.ford_joint.inverse import static_pair, quantize + +ARRAY = np.ctypeslib.ndpointer(dtype=np.float64, flags='C_CONTIGUOUS') + + +def state(m): + return np.array([m.c0, m.c1, m.c0_i, m.filtered, float(m.fast)], dtype=np.float64) + + +def parameters(m, speed, freeze_i=True, interaction=1.0): + if m.c2 != 0 or m.c3 != 0: + raise ValueError('C2/C3 must be zero') + if abs(m.preview_t - m.cal.f(0xFEF25A58)) > 1e-9: + raise ValueError('Fixed preview time required') + c = m.cal + probe = copy.copy(m).step(speed, m.c0, m.c1, freeze_i=freeze_i, interaction=interaction) + si, sf = probe['speed_index'], probe['speed_fraction'] + return np.array( + [ + probe['g0'], + probe['g1'], + interp_int(m.tables[0xFEF258FC], si, sf) / 256, + c.b(0xFEF25AA5) / 128, + c.b(0xFEF25AA6) / 128, + c.b(0xFEF25AA7) / 256, + c.b(0xFEF25AA4) / 256, + speed * 0.28, + c.f(0xFEF259F4), + interaction, + c.f(0xFEF259F8), + c.f(0xFEF259FC), + c.f(0xFEF25A08), + c.f(0xFEF25A0C), + c.f(0xFEF25A00), + c.f(0xFEF25A10), + c.h(0xFEF25A70) / 8192, + c.h(0xFEF25A72) / 8192, + 0.0 if freeze_i else interp(m.tables[0xFEF25B78], si, sf) * DT, + c.f(0xFEF25968), + c.f(0xFEF25960), + c.f(0xFEF25964), + c.f(0xFEF2595C), + interp_int(m.tables[0xFEF25C60], si, sf) / 512 * DT, + ], + dtype=np.float64, + ) + + +LIB = ctypes.CDLL(str(Path(__file__).with_name('libencoder' + ('.dylib' if sys.platform == 'darwin' else '.so')))) +LIB.paired_cost.argtypes = [ARRAY, ARRAY, ctypes.c_double, ARRAY, ctypes.c_int, ctypes.c_double, ctypes.c_double, ARRAY] +LIB.paired_cost.restype = ctypes.c_double +LIB.paired_select.argtypes = [ARRAY, ARRAY, ctypes.c_double, ARRAY, ctypes.c_int, ARRAY, ctypes.c_int, ARRAY, ctypes.c_int, ctypes.c_int, ARRAY] +LIB.paired_select.restype = None + + +def levels(held, pref, rate, count, lsb, bound, clear): + reach = rate * 0.008 * count + lsb / 2 + lo = max(round(-bound / lsb), int(np.floor((held - reach) / lsb))) + hi = min(round(bound / lsb), int(np.ceil((held + reach) / lsb))) + # Include both sides of the fast-latch clearing thresholds even when outside + # the reachable range: equal slew endpoints can otherwise have different flags. + near = np.floor(np.array([-clear, clear]) / lsb).astype(int) + thresholds = np.r_[near - 1, near, near + 1] * lsb + return np.unique(np.clip(np.r_[np.arange(lo, hi + 1) * lsb, pref, round(held / lsb) * lsb, thresholds, -bound, bound], -bound, bound)) + + +class PairedRelease: + def __init__(self, request, freeze_i=True, interaction=1.0, preserve_now=True): + self.request = request + self.freeze_i = freeze_i + self.interaction = interaction + self.preserve_now = preserve_now + + def choose(self, speed_kmh, curvature, phase=0): + m = self.request + if not self.freeze_i or m.c0_i != 0: + raise ValueError('Joint encoder requires its nominal zero-I estimate') + if phase not in range(10) or speed_kmh <= 0 or not np.isfinite([speed_kmh, curvature, *state(m)]).all(): + raise ValueError('Finite moving-vehicle inputs and scheduler phase required') + p = parameters(m, speed_kmh, True, self.interaction) + s = state(m) + pref = np.array(quantize(static_pair(m, curvature, speed_kmh))) + target = float(p[0] * pref[0] + p[1] * pref[1]) + count = next(k for k in range(1, 3) if (phase + 8 * k) // 10 > 0) + c0s = levels(m.c0, pref[0], max(p[10:12]), count, 0.01, 5.11, p[14]) + c1s = levels(m.c1, pref[1], max(p[12:14]), count, 0.0005, 0.5, p[15]) + result = np.full(9, np.nan) + LIB.paired_select(s, p, target, pref, count, c0s, len(c0s), c1s, len(c1s), int(self.preserve_now), result) + if not np.isfinite(result).all() or abs(result[0]) > 5.11 or abs(result[1]) > 0.5: + raise ValueError('No finite bounded joint command') + return tuple(result[:2]), { + 'cost': float(result[2]), + 'first_state': result[3:8].copy(), + 'count': count, + 'pref': pref, + 'planned_target': target, + 'evaluations': len(c0s) * len(c1s), + 'immediate_error_bound': result[8], + } diff --git a/openpilot/selfdrive/controls/lib/ford_joint/inverse.py b/openpilot/selfdrive/controls/lib/ford_joint/inverse.py new file mode 100644 index 0000000000..fd9dce61da --- /dev/null +++ b/openpilot/selfdrive/controls/lib/ford_joint/inverse.py @@ -0,0 +1,80 @@ +"""Invert the recovered angle-target stage; no torque or wheel plant model.""" + +import copy +import math + +from openpilot.selfdrive.controls.lib.ford_joint.model import clip, interp_int, bracket + +C0_BOUND = 5.11 +C1_BOUND = 0.5 + + +def invert_angle(output, speed_kmh, target_angle, angle, yaw, accel, wheelbase, ratio, accel_allowance=3.0): + """Invert 0x1818e6 BEFORE its separate angle-reference filter. + + The angle equation is affine in acceleration within the acceleration bounds. + Predict only the measurement-driven states it updates this cycle. The unknown + assist/load input occurs downstream and does not enter this calculation. + """ + if not all(math.isfinite(x) for x in (speed_kmh, target_angle, angle, yaw, accel, wheelbase, ratio)): + raise ValueError('Nonfinite input') + if speed_kmh <= 0 or wheelbase <= 0 or ratio <= 0: + raise ValueError('A moving vehicle with positive geometry is required') + c = output.cal + si, sf = bracket(int(speed_kmh * 256) & 0xFFFF, output.speed_bp) + + def tab(a): + return interp_int(output.tables[a], si, sf) + + v = speed_kmh / 3.6 + yaw_acc = output.yaw_acc + c.h(0xFEF25A90) / 65536 * (v * yaw - output.yaw_acc) + bank = output.bank_residual + c.h(0xFEF25A88) / 65536 * (accel - v * yaw - output.bank_residual) + floor_v = max(v, c.b(0xFEF25AB7) / 32, 1e-6) + understeer = c.read(0xFEF25A9A, 'h')[0] / 16384 + conversion = 180 / math.pi * ratio + bias_error = (floor_v * understeer * yaw + yaw / floor_v * wheelbase) * conversion - angle - output.angle_bias + bias = output.angle_bias + tab(0xFEF25CB0) / 65536 * bias_error + p = tab(0xFEF25CA0) / 2048 + slope = (c.b(0xFEF25AB1) / 128 + p) * (understeer + wheelbase / floor_v**2) * conversion + intercept = (-p * yaw_acc * (understeer + wheelbase / floor_v**2) + bank * c.read(0xFEF25AB0, 'b')[0] / 64 * understeer) * conversion + intercept += bias * c.read(0xFEF25AB2, 'b')[0] / 64 + def sign(x): + return int(x > 0) - int(x < 0) + allowance = accel_allowance + (abs(bank) if sign(yaw_acc) != sign(bank) else 0) + wanted_accel = (target_angle - intercept) / slope + bounded_accel = clip(wanted_accel, -allowance, allowance) + return { + 'curvature': bounded_accel / v**2, + 'wanted_accel': wanted_accel, + 'bounded_accel': bounded_accel, + 'allowance': allowance, + 'reachable_target': intercept + slope * bounded_accel, + 'accel_limited': abs(wanted_accel) > allowance, + 'slope_per_curvature': slope * v**2, + 'intercept': intercept, + } + + +def arc_pair(curvature, speed_kmh): + """Current base geometry only; no current OP proportional/integral terms.""" + half = 0.5 * 7 * curvature + sinc = math.sin(half) / half if half else 1.0 + c1 = max(7.0, speed_kmh / 3.6) * curvature + clipped_c1 = clip(c1, -C1_BOUND, C1_BOUND) + c0 = 0.5 * curvature * 49 * sinc * sinc + 7 * (c1 - clipped_c1) + return clip(c0, -C0_BOUND, C0_BOUND), clipped_c1 + + +def static_pair(request, curvature, speed_kmh): + """Preserve the base's channel proportion, solve its static curvature sum.""" + probe = copy.copy(request) + gains = probe.step(speed_kmh, request.c0, request.c1, freeze_i=True) + p0, p1 = arc_pair(curvature, speed_kmh) + total = gains['g0'] * p0 + gains['g1'] * p1 + scale = curvature / total if total else 1.0 + return clip(p0 * scale, -C0_BOUND, C0_BOUND), clip(p1 * scale, -C1_BOUND, C1_BOUND) + + +def quantize(pair): + """Existing symmetric controller bounds; DBC LSBs, round nearest.""" + return (clip(round(pair[0] / 0.01) * 0.01, -C0_BOUND, C0_BOUND), clip(round(pair[1] / 0.0005) * 0.0005, -C1_BOUND, C1_BOUND)) diff --git a/openpilot/selfdrive/controls/lib/ford_joint/model.py b/openpilot/selfdrive/controls/lib/ford_joint/model.py new file mode 100644 index 0000000000..35326f3055 --- /dev/null +++ b/openpilot/selfdrive/controls/lib/ford_joint/model.py @@ -0,0 +1,179 @@ +"""Nominal older-PSCM request recurrence for the opt-in joint encoder. + +The held fields, filter, slow/fast latch and zero internal I are estimates. +Calibration is packaged numerical data, never loaded from an ECU image. +""" + +import bisect +import json +import struct +from pathlib import Path + +DT = 0.008 + + +class Calibration: + def __init__(self): + self.entries = json.loads(Path(__file__).with_name('calibration.json').read_text())['entries'] + + def read(self, address, fmt='f'): + item = self.entries[hex(address)] + if item['format'] != fmt: + raise ValueError(f'Unexpected calibration format: {address:x}') + return tuple(item['values']) + + def f(self, address): + return self.read(address)[0] + + def b(self, address): + return self.read(address, 'B')[0] + + def h(self, address): + return self.read(address, 'H')[0] + + +def clip(x, lo, hi): + return min(max(x, lo), hi) + + +def slew(x, target, rate): + return x + clip(target - x, -rate * DT, rate * DT) + + +def bracket(x, breakpoints): + i = max(0, bisect.bisect_right(breakpoints, x) - 1) + if i == len(breakpoints) - 1 or x <= breakpoints[0]: + return i, 0 + return i, min(255, int((x - breakpoints[i]) * 256 / (breakpoints[i + 1] - breakpoints[i]))) + + +def interp(tab, idx, frac): + return tab[idx] if not frac else tab[idx] + (tab[idx + 1] - tab[idx]) * frac / 256 + + +def interp_int(tab, idx, frac): + if not frac: + return tab[idx] + d = tab[idx + 1] - tab[idx] + return tab[idx] + (1 if d >= 0 else -1) * (abs(d) * frac // 256) + + +def f32(x): + return struct.unpack(' c.h(0xFEF25A70) / 8192 and abs(self.c0_i) > c.h(0xFEF25A72) / 8192 and self.c0 * self.c0_i < 0 + forced_release = special_release or opposite + if active and not release_i and not forced_release: + delta = 0 if freeze_i else clip(self.c0, c.f(0xFEF25968), c.f(0xFEF25960)) * interp(self.tables[0xFEF25B78], si, sf) + self.c0_i = clip(self.c0_i + delta * DT, c.f(0xFEF25964), c.f(0xFEF2595C)) + else: + rate = (c.h(0xFEF25A74) if special_release else interp_int(self.tables[0xFEF25C60], si, sf)) if forced_release else c.h(0xFEF25A60) + self.c0_i = slew(self.c0_i, 0, rate / 512) + req = g3 * self.c3 + g1 * self.c1 + g0 * (self.c0 + self.c0_i) + self.c2 + + def contribution(lo, hi, x): + return clip(abs(x) - lo, 0, max(1e-6, hi - lo)) / max(1e-6, hi - lo) + + v = speed_kmh * 0.28 + heading_weight = contribution(c.b(0xFEF25AA5) / 128, c.b(0xFEF25AA6) / 128, self.c1 * v) + curvature_weight = contribution(c.b(0xFEF25AA1) / 64, c.b(0xFEF25AA2) / 64, self.c2 * v * v) + rate_weight = contribution(c.b(0xFEF25A9E) / 32, c.b(0xFEF25A9F) / 32, self.c3 * v * v * v) + alpha = min( + c.b(0xFEF25AA4) / 256, + (interp_int(self.tables[0xFEF258FC], si, sf) + c.b(0xFEF25AA7) * heading_weight + c.b(0xFEF25AA3) * curvature_weight + c.b(0xFEF25AA0) * rate_weight) + / 256, + ) + self.filtered += alpha * (req - self.filtered) + return { + 'held_c0': self.c0, + 'held_c1': self.c1, + 'held_c2': self.c2, + 'held_c3': self.c3, + 'integral_c0': self.c0_i, + 'request_curvature': req, + 'filtered_curvature': self.filtered, + 'preview_distance': distance, + 'preview_time': self.preview_t, + 'fast_flag': int(self.fast), + 'speed_index': si, + 'speed_fraction': sf, + 'alpha': alpha, + 'g0': g0, + 'g1': g1, + 'accel_request': self.filtered * (speed_kmh / 3.6) ** 2, + 'accel_bound': interp_int(self.tables[0xFEF25E20], si, sf) / 8192, + 'c0_contribution': g0 * self.c0, + 'c1_contribution': g1 * self.c1, + 'c0_i_contribution': g0 * self.c0_i, + } diff --git a/openpilot/selfdrive/controls/lib/ford_model_action.py b/openpilot/selfdrive/controls/lib/ford_model_action.py index 807c3e4c39..d7361a3a3d 100644 --- a/openpilot/selfdrive/controls/lib/ford_model_action.py +++ b/openpilot/selfdrive/controls/lib/ford_model_action.py @@ -206,7 +206,11 @@ class FordModelActionController: neither a limit nor a repeated measurement freezes the model request. """ def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False, - c0_proportional_gain=None, direct_path=False): + c0_proportional_gain=None, direct_path=False, joint_control=False): + self.joint_control = bool(joint_control and not direct_path) + if self.joint_control: + proportional_gain = integral_gain = c0_proportional_gain = 0. + c0_time_based = False if c0_proportional_gain is None: c0_proportional_gain = DIRECT_PATH_C0_PROPORTIONAL_GAIN if direct_path else C0_PROPORTIONAL_GAIN self.core = ModelActionController(proportional_gain=proportional_gain, integral_gain=integral_gain, c0_time_based=c0_time_based, @@ -216,6 +220,8 @@ class FordModelActionController: self.request_buffer = deque([0.] * self.request_buffer_size, maxlen=self.request_buffer_size) self.hypothesis = ('model-path-direct-feedback-v23' if self.direct_path else 'model-action-curvature-c0-feedback-v23-soft-c0-c1') + if self.joint_control: + self.hypothesis = 'model-action-joint-reference-v24' self.reset() def path_curvature(self, model, speed): @@ -225,7 +231,7 @@ class FordModelActionController: 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): + if self.joint_control or lateral_engaged or self.core.c0_time_based == bool(enabled): return False self.core.c0_time_based = bool(enabled) self.reset('c0_distance_changed') @@ -312,13 +318,18 @@ class FordModelActionController: 'feedback_error': self.core.feedback_curvature-current_curvature, 'driver_override': driver_override, 'pscm_limited': pscm_limited, 'pscm_status_fresh': bool(status_fresh), 'command': (command.path_offset, command.path_angle, 0., 0.)} + if self.joint_control: + # card receives the normal desired steering angle and owns allocation at + # the transmit boundary. This path carries validity, never a second PI. + self.diagnostics['command_stage'] = 'reference_only' + return FordPath(valid=True) return command -def select_model_action_controller(CP, enabled, *, c0_time_based=False, direct_path=False): +def select_model_action_controller(CP, enabled, *, c0_time_based=False, direct_path=False, joint_control=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, - c0_time_based=c0_time_based, direct_path=direct_path) + c0_time_based=c0_time_based, direct_path=direct_path, joint_control=joint_control) return None diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/toggles.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/toggles.py index b6b7f1bebc..a4ad23bef3 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/toggles.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/toggles.py @@ -21,6 +21,7 @@ class TogglesLayoutMiciSP(TogglesLayoutMici): 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')) + and ui_state.params.get_bool('FordModelActionController') and + (not ui_state.params.get_bool('FordPscmJointControl') or ui_state.params.get_bool('FordGeometryReference'))) self._ford_c0_toggle.set_visible(visible) self._ford_c0_help.set_visible(visible) diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index f4e55a13c4..345cb708dd 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -2208,6 +2208,29 @@ "equals": true } ] + }, + { + "key": "FordPscmJointControl", + "widget": "toggle", + "needs_onroad_cycle": true, + "title": "Coordinated C0/C1 Steering (Experimental)", + "description": "Coordinate steering entry and release using an estimated Ford steering-controller response.", + "details": "Uses the normal model-action steering target. Requires Selected-Action Path Tracking on and Model Geometry Reference off. Default off; turning this option off restores the existing selected-action controller. Turning Selected-Action Path Tracking off restores upstream Ford control. Changes apply after an offroad-to-onroad cycle. This trial uses a response reconstructed from older Ford firmware and has only been tested offline. C0 distance selection does not apply to this trial.", + "enablement": [ + { + "type": "offroad_only" + }, + { + "type": "param", + "key": "FordModelActionController", + "equals": true + }, + { + "type": "param", + "key": "FordGeometryReference", + "equals": false + } + ] } ] }, diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml index e7fa8eb5b3..d7b2eda7ed 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml @@ -29,6 +29,20 @@ sections: - type: param key: FordModelActionController equals: true + - key: FordPscmJointControl + widget: toggle + needs_onroad_cycle: true + title: Coordinated C0/C1 Steering (Experimental) + description: Coordinate steering entry and release using an estimated Ford steering-controller response. + details: Uses the normal model-action steering target. Requires Selected-Action Path Tracking on and Model Geometry Reference off. Default off; turning this option off restores the existing selected-action controller. Turning Selected-Action Path Tracking off restores upstream Ford control. Changes apply after an offroad-to-onroad cycle. This trial uses a response reconstructed from older Ford firmware and has only been tested offline. C0 distance selection does not apply to this trial. + enablement: + - $ref: '#/macros/offroad' + - type: param + key: FordModelActionController + equals: true + - type: param + key: FordGeometryReference + equals: false - id: hyundai title: Hyundai / Kia / Genesis Settings description: '' diff --git a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py index 84fc0be89f..e0c7895f17 100644 --- a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py +++ b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py @@ -307,6 +307,16 @@ class TestKnownVehicleSettings(OpenpilotTestCase): with tempfile.TemporaryDirectory() as path: assert Params(path).get_default_value("FordGeometryReference") is False + def test_ford_joint_is_default_off_and_requires_action_mode(self, schema): + items = _brand_items(schema["vehicle_settings"].get("ford")) + item = next(item for item in items if item["key"] == "FordPscmJointControl") + assert item["widget"] == "toggle" and item["needs_onroad_cycle"] is True + assert item["enablement"] == [{"type": "offroad_only"}, + {"type": "param", "key": "FordModelActionController", "equals": True}, + {"type": "param", "key": "FordGeometryReference", "equals": False}] + with tempfile.TemporaryDirectory() as path: + assert Params(path).get_default_value("FordPscmJointControl") is False + def test_hyundai_has_longitudinal_tuning(self, schema): keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("hyundai"))} assert "HyundaiLongitudinalTuning" in keys