Ford: soften small C0 feedback corrections

Reduce incremental C0 proportional gain near zero error, smoothly returning toward the existing slope for large errors. Preserve C1 feedback, request timing, base mapping, field bounds, and the upstream fallback.

Validate with 719 tests and 25 subtests, plus five route replays covering 646178 cycles and 64620 CAN round trips. Replay confirms command behavior; physical stability remains an onroad trial.

docs/ford_c0_softening_v22.md

@@ -0,0 +1,85 @@
+# Action-mode C0 feedback softening
+
+Route 166 used v21 and improved normal driving, but the first two user bookmarks
+showed roughly 2 Hz wheel oscillations around a smoother requested angle at
+17–31 mph. The first occurred with uninterrupted feedback, small I and no
+PSCM limitReached. Both C0 P and C1 P followed the error, so these recordings
+do not isolate either channel's contribution to physical oscillation.
+
+This trial changes C0 P only. With the curvature error converted into `x` metres
+of C0 before applying the existing gain, the new correction is:
+
+```text
+C0 P = gain * (x - 0.125 * tanh(x / 0.25))
+```
+
+Near zero, the slope is half the old gain. It increases smoothly toward the old
+gain without exceeding it. The response stays symmetric, monotonic and nonzero
+for nonzero error; it introduces no time filter, deadband, cutoff or additional
+state in the control law. Zero error removes P immediately, and opposite error
+commands opposite P in the same cycle.
+
+Large corrections lose at most 0.125 m times the existing C0 gain. This is a
+bound on the difference from the old correction, **not a C0 command cap**.
+The field bounds remain C0 ±5.11 m and C1 ±0.5 rad. The scale and minimum slope
+are explicit drive-trial choices, not an identified stable PSCM calibration.
+Scales 0.15, 0.25, 0.4 and 0.5 m were compared on route 166; 0.25 m preserves
+about 96% of the good left's peak entry command while reducing the repeated
+C0 command component in both wobbles.
+
+The base action mapping, selected desired curvature, C1 P/I, delayed reference,
+integral cadence and anti-windup, driver/PSCM arbitration, CAN cadence and
+upstream limits are unchanged. The softening applies at all speeds in action
+mode: the bookmarked wobbles were above the proposed 11.2 mph freeze cutoff.
+Direct-path feedback retains its linear law; when it falls back to an action
+reference, the action softening applies. The master toggle still selects
+upstream Ford control when disabled. No new toggle is added.
+
+Diagnostics identify v22 and include `offset_proportional_linear` alongside
+the actual `offset_proportional`, so a new log can show exactly what softening
+removed. The extra stored scalar is diagnostic only and resets with P.
+
+## Validation
+
+Five native-time route replays cover 646,178 cycles and 64,620 CAN encode/decode
+checks: routes 149, 151, 157, 162 and 166. The v21 baseline matches its archived
+commands and integral exactly. C1 commands, C1 P/I, reference, validity,
+arbitration and base mapping remain identical on every cycle. C2/C3 remain
+zero. The largest raw C0 P change is 0.125 m; after CAN quantization the
+command difference is at most 0.13 m.
+
+| Route-166 event | Repeating total C0 component, before → after | Reduction |
+| --- | --- | --- |
+| First wobble | 0.146 → 0.076 m peak-to-peak | 48% |
+| Second wobble | 0.222 → 0.150 m peak-to-peak | 32% |
+| Second wobble exit | 0.335 → 0.199 m peak-to-peak | 41% |
+
+These use a same-frequency sine fit with quadratic trend removal. They measure
+the **command**, not a predicted reduction in wheel oscillation. The good left's
+peak entry C0 changes from -2.93 to -2.81 m; its mid-turn unwind peak changes
+from +1.44 to +1.31 m. C1 is identical. Entry and unwind are both softened;
+neither physical response is proven better by a frozen-motion replay.
+
+Regression tests first failed against v21, then passed with v22. They exercise
+small nonzero corrections, preserved large-error authority, monotonic bounded
+incremental gain, immediate reversal/release, unaffected C1 and base mapping,
+and the direct-path exception. Existing controls-to-CAN integration, model
+selection, driver override, request-history timing and default-upstream tests
+also pass. Exact counts and provenance are in
+`ford_c0_softening_v22_validation.json`.
+
+Reproduce a route comparison with the built Python dependencies:
+
+```sh
+PYTHONPATH=.:opendbc_repo:.cache/ford_geometry_deps python \
+  tools/ford_pscm_lab/c0_softening_replay.py \
+  --source .cache/ford_route166/full \
+  --delay-intake .cache/ford_route166/intake.npz \
+  --archive .cache/ford_feedback_delay_v21/166/commands.npz \
+  --output .cache/ford_c0_softening_v22/166
+```
+
+For the next drive, the discriminating observations are whether small
+left-right corrections settle sooner, whether centering becomes too loose,
+and whether strong entry and prompt release remain. Do not interpret this
+offline validation as a demonstrated physical stability fix.

docs/ford_c0_softening_v22_validation.json

@@ -0,0 +1,415 @@
+{
+  "baseline_commit": "ed9c44f57584b5f033f79db6331e21b9b3e0ad55",
+  "method": "Native-time production-controller replay using recorded wheel motion. Tests command behavior, not changed physical tracking.",
+  "cycles": 646178,
+  "wire_checks": 64620,
+  "tests": {
+    "ford_controller_and_integration": 708,
+    "subtests": 2,
+    "ford_can": 11,
+    "ruff": "passed",
+    "diff_check": "passed",
+    "ford_can_subtests": 23
+  },
+  "selection": {
+    "0.15": {
+      "wobble1": {
+        "P_periodic_reduction_pct": 43.471957098342315,
+        "P_before": 0.1502831327602382,
+        "P_after": 0.08495211376066261
+      },
+      "wobble2": {
+        "P_periodic_reduction_pct": 20.901758921591217,
+        "P_before": 0.22856483271408398,
+        "P_after": 0.1807907624006479
+      },
+      "wobble2_exit": {
+        "P_periodic_reduction_pct": 31.59900962734852,
+        "P_before": 0.3407619177930598,
+        "P_after": 0.2330845265832934
+      },
+      "left_entry_peak": {
+        "t": 557.084304569,
+        "before": -2.9300000000000006,
+        "after_unpacked": -2.8568021983086886,
+        "retained_percent": 97.50178151224192
+      }
+    },
+    "0.25": {
+      "wobble1": {
+        "P_periodic_reduction_pct": 47.14718986588794,
+        "P_before": 0.1502831327602382,
+        "P_after": 0.07942885882136426
+      },
+      "wobble2": {
+        "P_periodic_reduction_pct": 31.648695347911747,
+        "P_before": 0.22856483271408398,
+        "P_after": 0.1562270451359394
+      },
+      "wobble2_exit": {
+        "P_periodic_reduction_pct": 40.10137585312036,
+        "P_before": 0.3407619177930598,
+        "P_after": 0.2041117003745639
+      },
+      "left_entry_peak": {
+        "t": 557.084304569,
+        "before": -2.9300000000000006,
+        "after_unpacked": -2.8068022480277577,
+        "retained_percent": 95.79529856750024
+      }
+    },
+    "0.4": {
+      "wobble1": {
+        "P_periodic_reduction_pct": 48.78814118850091,
+        "P_before": 0.1502831327602382,
+        "P_after": 0.07696278576667091
+      },
+      "wobble2": {
+        "P_periodic_reduction_pct": 40.16673505205207,
+        "P_before": 0.22856483271408398,
+        "P_after": 0.13675780193565182
+      },
+      "wobble2_exit": {
+        "P_periodic_reduction_pct": 45.26729702228269,
+        "P_before": 0.3407619177930598,
+        "P_after": 0.18650820832684864
+      },
+      "left_entry_peak": {
+        "t": 557.084304569,
+        "before": -2.9300000000000006,
+        "after_unpacked": -2.7318281193111016,
+        "retained_percent": 93.23645458399662
+      }
+    },
+    "0.5": {
+      "wobble1": {
+        "P_periodic_reduction_pct": 49.20782335256252,
+        "P_before": 0.1502831327602382,
+        "P_after": 0.07633207426288317
+      },
+      "wobble2": {
+        "P_periodic_reduction_pct": 43.09149572817541,
+        "P_before": 0.22856483271408398,
+        "P_after": 0.1300728275889832
+      },
+      "wobble2_exit": {
+        "P_periodic_reduction_pct": 46.79902581397264,
+        "P_before": 0.3407619177930598,
+        "P_after": 0.18128865992089754
+      },
+      "left_entry_peak": {
+        "t": 557.084304569,
+        "before": -2.9300000000000006,
+        "after_unpacked": -2.682025078944252,
+        "retained_percent": 91.53669211413828
+      }
+    }
+  },
+  "bookmarks": {
+    "wobble1": {
+      "window": [
+        420.8,
+        424.0
+      ],
+      "p": {
+        "old_peak_to_peak": 0.1502831327602382,
+        "new_peak_to_peak": 0.07942885882136426,
+        "reduction_percent": 47.14718986588794
+      },
+      "c0": {
+        "old_peak_to_peak": 0.1463643509329174,
+        "new_peak_to_peak": 0.07619232987207852,
+        "reduction_percent": 47.943382807061084
+      }
+    },
+    "wobble2": {
+      "window": [
+        514.3,
+        521.4
+      ],
+      "p": {
+        "old_peak_to_peak": 0.22856483271408398,
+        "new_peak_to_peak": 0.15622704513593932,
+        "reduction_percent": 31.648695347911794
+      },
+      "c0": {
+        "old_peak_to_peak": 0.22189262078364064,
+        "new_peak_to_peak": 0.15013115100291197,
+        "reduction_percent": 32.3406292319656
+      }
+    },
+    "wobble2_exit": {
+      "window": [
+        523.7,
+        526.8
+      ],
+      "p": {
+        "old_peak_to_peak": 0.3407619177930598,
+        "new_peak_to_peak": 0.2041117003745639,
+        "reduction_percent": 40.10137585312036
+      },
+      "c0": {
+        "old_peak_to_peak": 0.3347422828817533,
+        "new_peak_to_peak": 0.19869356211391034,
+        "reduction_percent": 40.642825159886286
+      }
+    }
+  },
+  "good_left_points": [
+    {
+      "meaning": "entry",
+      "t": 557.084304569,
+      "target_angle": 167.83670043945312,
+      "actual_angle": 76.5999984741211,
+      "old_c0": -2.9300000000000006,
+      "new_c0": -2.8100000000000005,
+      "old_p": -1.9288202876424623,
+      "new_p": -1.8038203373625472,
+      "c1": -0.4565,
+      "i": -0.058477505092137665
+    },
+    {
+      "meaning": "midturn_unwind",
+      "t": 558.284113233,
+      "target_angle": 109.08927154541016,
+      "actual_angle": 183.6999969482422,
+      "old_c0": 1.44,
+      "new_c0": 1.31,
+      "old_p": 2.0905105735349747,
+      "new_p": 1.9655105871733405,
+      "c1": -0.11499999999999999,
+      "i": -0.05363273646512535
+    }
+  ],
+  "routes": {
+    "166": {
+      "cycles": 62579,
+      "wire_checks": 6258,
+      "baseline_matches_archived_v21_exactly": true,
+      "c1_integral_reference_arbitration_equal_every_cycle": true,
+      "P_reduction_p50_p95_p99_max_m": [
+        0.003636227080371235,
+        0.10752626130792256,
+        0.12490297403763004,
+        0.12499999961974328
+      ],
+      "C0_change_p50_p95_p99_max_m": [
+        0.0,
+        0.10999999999999943,
+        0.1299999999999999,
+        0.13000000000000078
+      ],
+      "variants": {
+        "old": {
+          "c0_step_p95_p99_max": [
+            0.020000000000000018,
+            0.050000000000000266,
+            1.8900000000000006
+          ],
+          "c0_bound_seconds": 0.0
+        },
+        "new": {
+          "c0_step_p95_p99_max": [
+            0.010000000000000675,
+            0.040000000000000036,
+            1.7700000000000005
+          ],
+          "c0_bound_seconds": 0.0
+        }
+      },
+      "baseline_commit": "ed9c44f57584b5f033f79db6331e21b9b3e0ad55",
+      "method": "Replay C0 softening against v21 on frozen recorded motion, not a PSCM model.",
+      "controller_sha256": "b0aff6d70ad8ae7693c69068dbe646767e4d9fc20a23af70cbddcf6ebf849f48",
+      "source_sha256": {
+        ".cache/ford_route166/full/route.npz": "0ce273e4a12725c4481b11dab38b304950d364455b89f216b929a9c61b7feb1c",
+        ".cache/ford_route166/full/model_paths.npz": "720fe9ba9865a05937989449b0b0bd3304baf5f1730042da6b61adc2941c0f5a",
+        ".cache/ford_route166/intake.npz": "660b5e7f6789d931660ad71b10f7d8ce2b63436e7573b615ccc8e5da1f2a14d0",
+        ".cache/ford_feedback_delay_v21/166/commands.npz": "bfe51abd9e914edea505efc4fb42fe66950cfa5835e4f929d922992e90c88b5b"
+      }
+    },
+    "162": {
+      "cycles": 49003,
+      "wire_checks": 4901,
+      "baseline_matches_archived_v21_exactly": true,
+      "c1_integral_reference_arbitration_equal_every_cycle": true,
+      "P_reduction_p50_p95_p99_max_m": [
+        0.01864462197101791,
+        0.12428797325001033,
+        0.12499916992638829,
+        0.12499999992682431
+      ],
+      "C0_change_p50_p95_p99_max_m": [
+        0.019999999999999574,
+        0.1200000000000001,
+        0.1299999999999999,
+        0.13000000000000078
+      ],
+      "variants": {
+        "old": {
+          "c0_step_p95_p99_max": [
+            0.03000000000000025,
+            0.09999999999999964,
+            2.7299999999999995
+          ],
+          "c0_bound_seconds": 0.0
+        },
+        "new": {
+          "c0_step_p95_p99_max": [
+            0.03000000000000025,
+            0.08999999999999986,
+            2.5999999999999996
+          ],
+          "c0_bound_seconds": 0.0
+        }
+      },
+      "baseline_commit": "ed9c44f57584b5f033f79db6331e21b9b3e0ad55",
+      "method": "Replay C0 softening against v21 on frozen recorded motion, not a PSCM model.",
+      "controller_sha256": "b0aff6d70ad8ae7693c69068dbe646767e4d9fc20a23af70cbddcf6ebf849f48",
+      "source_sha256": {
+        ".cache/ford_route162/full/route.npz": "a725f9ca309b90a2d750d51a9b9d5b459d99119a54ab048631bc0fce07348af8",
+        ".cache/ford_route162/full/model_paths.npz": "cffe310b122d9ae26a53ee504608156b4242a6bba5f681898d5b661951ac5ce5",
+        ".cache/ford_route162/intake.npz": "9455b01e1d1d3dcba08276b382a8c6cd54ff6fb19cb417b72d0ac89cf193034b",
+        ".cache/ford_feedback_delay_v21/162/commands.npz": "6c70518fb9b7232b689740662c4bd5c0c24cabee4e743a2b4f3b2313bcce1912"
+      }
+    },
+    "157": {
+      "cycles": 76554,
+      "wire_checks": 7656,
+      "baseline_matches_archived_v21_exactly": true,
+      "c1_integral_reference_arbitration_equal_every_cycle": true,
+      "P_reduction_p50_p95_p99_max_m": [
+        0.00530841320802854,
+        0.12371750128642126,
+        0.12499999318487974,
+        0.125
+      ],
+      "C0_change_p50_p95_p99_max_m": [
+        0.009999999999999787,
+        0.1200000000000001,
+        0.1299999999999999,
+        0.13000000000000078
+      ],
+      "variants": {
+        "old": {
+          "c0_step_p95_p99_max": [
+            0.03000000000000025,
+            0.09000000000000014,
+            5.6000000000000005
+          ],
+          "c0_bound_seconds": 0.2999506119999751
+        },
+        "new": {
+          "c0_step_p95_p99_max": [
+            0.02999999999999936,
+            0.08000000000000007,
+            5.6000000000000005
+          ],
+          "c0_bound_seconds": 0.15561390599987135
+        }
+      },
+      "baseline_commit": "ed9c44f57584b5f033f79db6331e21b9b3e0ad55",
+      "method": "Replay C0 softening against v21 on frozen recorded motion, not a PSCM model.",
+      "controller_sha256": "b0aff6d70ad8ae7693c69068dbe646767e4d9fc20a23af70cbddcf6ebf849f48",
+      "source_sha256": {
+        ".cache/ford_route157/full/route.npz": "e2e2573f904aa11ee9e10450e7f5b965d475657b61127e827a67eadcd6b857fb",
+        ".cache/ford_route157/full/model_paths.npz": "25fc2526e092fde5ceb5aab63a5aa4f73a2c989a3263cedb0a26faf8da61c47b",
+        ".cache/ford_route157/intake.npz": "5dffd86fde68197727d6c6b9c5eae82320a456fa1206c4cf18e5e9c69419dde4",
+        ".cache/ford_feedback_delay_v21/157/commands.npz": "7d7659c54662e7f56015313e634b959be29dae524522b425f4c2adccc391fc9d"
+      }
+    },
+    "151": {
+      "cycles": 325708,
+      "wire_checks": 32571,
+      "baseline_matches_archived_v21_exactly": true,
+      "c1_integral_reference_arbitration_equal_every_cycle": true,
+      "P_reduction_p50_p95_p99_max_m": [
+        0.004569097471431923,
+        0.07115894622455567,
+        0.12458615764509068,
+        0.12499999999990274
+      ],
+      "C0_change_p50_p95_p99_max_m": [
+        0.009999999999999787,
+        0.07000000000000028,
+        0.1200000000000001,
+        0.13000000000000078
+      ],
+      "variants": {
+        "old": {
+          "c0_step_p95_p99_max": [
+            0.010000000000000675,
+            0.040000000000000036,
+            3.45
+          ],
+          "c0_bound_seconds": 0.0
+        },
+        "new": {
+          "c0_step_p95_p99_max": [
+            0.010000000000000231,
+            0.03000000000000025,
+            3.33
+          ],
+          "c0_bound_seconds": 0.0
+        }
+      },
+      "baseline_commit": "ed9c44f57584b5f033f79db6331e21b9b3e0ad55",
+      "method": "Replay C0 softening against v21 on frozen recorded motion, not a PSCM model.",
+      "controller_sha256": "b0aff6d70ad8ae7693c69068dbe646767e4d9fc20a23af70cbddcf6ebf849f48",
+      "source_sha256": {
+        ".cache/ford_route151/full/route.npz": "41a5b8bd388cf5a3d553f784542376ac9355fcdc5be4f427053d0504537babe1",
+        ".cache/ford_route151/full/model_paths.npz": "980b3843cec04254280d744a5801cee1bf0f8ef371052398327ff245f9eee01b",
+        ".cache/ford_route151/intake.npz": "18bbefb7738cebd0071dc987e90f74469a0c8ed456f9412324030592cafd68c9",
+        ".cache/ford_feedback_delay_v21/151/commands.npz": "4ab25e4c31fcddf08184c396efc3db78953d5ce60da7c653964bc2fa7e3184c9"
+      }
+    },
+    "149": {
+      "cycles": 132334,
+      "wire_checks": 13234,
+      "baseline_matches_archived_v21_exactly": true,
+      "c1_integral_reference_arbitration_equal_every_cycle": true,
+      "P_reduction_p50_p95_p99_max_m": [
+        0.010345416404636773,
+        0.12405821661525897,
+        0.12499991437968122,
+        0.12499999999999556
+      ],
+      "C0_change_p50_p95_p99_max_m": [
+        0.009999999999999787,
+        0.1200000000000001,
+        0.1299999999999999,
+        0.13000000000000078
+      ],
+      "variants": {
+        "old": {
+          "c0_step_p95_p99_max": [
+            0.03000000000000025,
+            0.08999999999999986,
+            3.87
+          ],
+          "c0_bound_seconds": 0.0
+        },
+        "new": {
+          "c0_step_p95_p99_max": [
+            0.020000000000000462,
+            0.08000000000000007,
+            3.750000000000001
+          ],
+          "c0_bound_seconds": 0.0
+        }
+      },
+      "baseline_commit": "ed9c44f57584b5f033f79db6331e21b9b3e0ad55",
+      "method": "Replay C0 softening against v21 on frozen recorded motion, not a PSCM model.",
+      "controller_sha256": "b0aff6d70ad8ae7693c69068dbe646767e4d9fc20a23af70cbddcf6ebf849f48",
+      "source_sha256": {
+        ".cache/ford_route149/full/route.npz": "aa5902877343cd033ee286b3668d91a85336ffbf740861849fb0b76b0ca24ade",
+        ".cache/ford_route149/full/model_paths.npz": "19827800b8fb5983f3d6b72fcfaf36e35a40170bb17cd7c1e49374744fb449fb",
+        ".cache/ford_route149/intake.npz": "503919c4f1566ef850007c59f16ce074697ba57732f21beb7b158c1ca7be3a9d",
+        ".cache/ford_feedback_delay_v21/149/commands.npz": "15c4ad68338a921cd6b2cd22d0b3d6b5b8637614dacbe61f954a3a6d7cb8d8d9"
+      }
+    }
+  },
+  "code_sha256": {
+    "openpilot/selfdrive/controls/lib/ford_model_action.py": "b0aff6d70ad8ae7693c69068dbe646767e4d9fc20a23af70cbddcf6ebf849f48",
+    "tools/ford_pscm_lab/c0_softening_replay.py": "43e185871e5b9875cee5288160d344fe4d606c4bdf63dd719fb6a6cd9404307e"
+  }
+}

openpilot/selfdrive/controls/lib/ford_model_action.py

@@ -1,7 +1,8 @@
 """Opt-in Ford C2-free model mapping with measured-curvature PI feedback.
 
 C0 samples a desired-curvature arc at 7 m, optionally max(7 m, v*1s),
-including base-heading overflow, plus proportional tracking correction. C1
+including base-heading overflow, plus proportional tracking correction softened
+near zero error without filtering or a deadband. 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.
@@ -26,6 +27,10 @@ C1_PROPORTIONAL_GAIN = 0.75  # Drive-trial gains, not a learned calibration.
 C1_INTEGRAL_GAIN = 1.0
 C0_PROPORTIONAL_GAIN = 1.0  # Action trial: stronger immediate correction for the same tracking error.
 DIRECT_PATH_C0_PROPORTIONAL_GAIN = 0.5
+# Action-only trial: soften the error expressed as metres of C0 before gain.
+# The slope grows smoothly from 0.5 to 1; correction reduction is at most 0.125 m * gain.
+C0_SMALL_ERROR_GAIN = 0.5
+C0_SMALL_ERROR_SCALE_M = 0.25
 # Offline Lightning fit: geometric curvature per metre of C0, with v in m/s.
 C0_RESPONSE_CONSTANT = 0.010717679293424373
 C0_RESPONSE_INVERSE_SPEED_SQUARED = 0.018122981795212647
@@ -96,7 +101,7 @@ class ModelActionController:
   Freshness, measurement cadence and driver/PSCM arbitration belong to the caller.
   """
   __slots__ = ('c0', 'c1', 'correction', 'proportional_gain', 'integral_gain', 'proportional', 'feedback_curvature', 'c0_time_based',
-               'c0_proportional_gain', 'offset_proportional', 'offset_reference')
+               'c0_proportional_gain', 'offset_proportional', 'offset_proportional_linear', 'offset_reference')
 
   def __init__(self, proportional_gain=C1_PROPORTIONAL_GAIN, integral_gain=C1_INTEGRAL_GAIN, *, c0_time_based=False,
                c0_proportional_gain=C0_PROPORTIONAL_GAIN):
@@ -109,7 +114,7 @@ class ModelActionController:
 
   def reset(self):
     self.c0 = self.c1 = self.correction = self.proportional = self.feedback_curvature = 0.
-    self.offset_proportional = 0.
+    self.offset_proportional = self.offset_proportional_linear = 0.
     self.offset_reference = None
 
   def update(self, model, desired_curvature, *, current_curvature, speed, dt, active=True, valid=True,
@@ -147,10 +152,17 @@ class ModelActionController:
     # Road-curvature error -> geometric steering error -> metres of C0.
     # This is proportional only: nothing is accumulated or carried into a release.
     response = C0_RESPONSE_CONSTANT+C0_RESPONSE_INVERSE_SPEED_SQUARED/max(speed, 1.34)**2
-    self.offset_proportional = self.c0_proportional_gain*error*curvature_scale/response if feedback_enabled else 0.
+    offset_error = error*curvature_scale/response
+    self.offset_proportional_linear = self.c0_proportional_gain*error*curvature_scale/response if feedback_enabled else 0.
+    self.offset_proportional = self.offset_proportional_linear
     if not _finite(self.proportional, self.offset_proportional):
       self.reset()
       return FordPath()
+    if feedback_enabled and not direct_path:
+      # Unlike blending gains back to 1, this never introduces a slope above the
+      # original gain. Large corrections lose only a bounded amount, not a fraction.
+      self.offset_proportional -= self.c0_proportional_gain*(1.-C0_SMALL_ERROR_GAIN)*C0_SMALL_ERROR_SCALE_M*math.tanh(
+        offset_error/C0_SMALL_ERROR_SCALE_M)
     base = float(np.clip(target.path_angle, -.5, .5))
     offset = float(np.clip(target.path_offset+OFFSET_STATION_M*(target.path_angle-base), -5.11, 5.11))
     self.c0 = float(np.clip(offset+self.offset_proportional, -5.11, 5.11))
@@ -198,8 +210,8 @@ class FordModelActionController:
     self.direct_path = bool(direct_path)
     self.request_buffer_size = int(CURVATURE_REQUEST_BUFFER_SECONDS / DT_CTRL)
     self.request_buffer = deque([0.] * self.request_buffer_size, maxlen=self.request_buffer_size)
-    self.hypothesis = ('model-path-direct-feedback-v21-delayed-feedback' if self.direct_path else
-                       'model-action-curvature-c0-feedback-v21-delayed-feedback')
+    self.hypothesis = ('model-path-direct-feedback-v22-soft-c0' if self.direct_path else
+                       'model-action-curvature-c0-feedback-v22-soft-c0')
     self.reset()
 
   def path_curvature(self, model, speed):
@@ -288,6 +300,7 @@ class FordModelActionController:
                         'heading_feedforward': base_heading,
                         'offset_overflow': OFFSET_STATION_M*(raw_heading-base_heading),
                         'offset_proportional': self.core.offset_proportional, 'c0_proportional_gain': self.core.c0_proportional_gain,
+                        'offset_proportional_linear': self.core.offset_proportional_linear,
                         'curvature_scale': curvature_scale,
                         'heading_correction': self.core.correction, 'feedback_enabled': feedback_enabled,
                         'heading_proportional': self.core.proportional, 'proportional_gain': self.core.proportional_gain,

openpilot/selfdrive/controls/tests/test_ford_c0_softening.py

@@ -0,0 +1,83 @@
+"""C0 trial command properties; these tests do not simulate PSCM stability."""
+import math
+
+import numpy as np
+import pytest
+
+from openpilot.selfdrive.controls.lib.ford_model_action import (
+  C0_RESPONSE_CONSTANT, C0_RESPONSE_INVERSE_SPEED_SQUARED, ModelActionController,
+)
+from openpilot.selfdrive.controls.tests.test_ford_model_action import straight
+
+
+def correction_for_linear_request(controller, request, **overrides):
+  # Choose a curvature mismatch which previously produced `request` metres of P.
+  response = C0_RESPONSE_CONSTANT+C0_RESPONSE_INVERSE_SPEED_SQUARED/100.
+  controller.update(straight(), 0., current_curvature=-request*response, speed=10., dt=.01, feedback_dt=0., **overrides)
+  return controller.offset_proportional
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_small_c0_correction_is_gentler_but_has_no_deadband(sign):
+  core = ModelActionController()
+  for magnitude in [1e-9, .001, .01]:
+    correction = correction_for_linear_request(core, sign*magnitude)
+    assert .4999*magnitude <= sign*correction <= .501*magnitude
+  assert correction_for_linear_request(core, 0.) == 0.
+
+
+@pytest.mark.parametrize('sign', [-1., 1.])
+def test_large_entry_and_release_corrections_keep_strength(sign):
+  core = ModelActionController()
+  for magnitude in [1., 2., 5.]:
+    correction = correction_for_linear_request(core, sign*magnitude)
+    assert 0. < magnitude-sign*correction <= .125000001
+    if magnitude >= 2.:
+      assert sign*correction >= .9375*magnitude
+  # No filter history or threshold state delays a reversal or a release.
+  correction_for_linear_request(core, sign*2.)
+  assert correction_for_linear_request(core, -sign*.01)*sign < 0.
+  assert correction_for_linear_request(core, 0.) == 0.
+
+
+def test_softening_never_adds_a_transition_with_higher_incremental_gain():
+  core = ModelActionController()
+  inputs = np.linspace(-2., 2., 2001)
+  values = np.array([correction_for_linear_request(core, value) for value in inputs])
+  slopes = np.diff(values)/np.diff(inputs)
+  assert .499999 <= slopes.min() < .501
+  assert slopes.max() <= 1.000001
+  np.testing.assert_allclose(values, -values[::-1], atol=1e-12)
+  assert np.all(abs(values) <= abs(inputs)+1e-12)
+
+
+def test_shaping_c0_does_not_change_c1_or_integration():
+  core = ModelActionController()
+  without_c0 = ModelActionController(c0_proportional_gain=0.)
+  model = straight()
+  for i in range(600):
+    desired = .04*math.sin(i*.017)
+    args = {'current_curvature': .025*math.sin((i-16)*.017), 'speed': 10., 'dt': .01,
+            'feedback_enabled': i % 29 != 0, 'pscm_limited': i % 17 == 0}
+    first, second = [c.update(model, desired, **args) for c in (core, without_c0)]
+    assert first.path_angle == second.path_angle
+    assert core.correction == without_c0.correction
+    assert core.proportional == without_c0.proportional
+    assert first.curvature == first.curvature_rate == 0.
+
+
+@pytest.mark.parametrize('direct_path', [False, True])
+def test_softening_does_not_modify_the_base_or_override_behavior(direct_path):
+  core = ModelActionController()
+  without_c0 = ModelActionController(c0_proportional_gain=0.)
+  for feedback_enabled, desired, measured in [(True, .01, .01), (False, .01, 0.), (True, 0., 0.)]:
+    args = {'current_curvature': measured, 'speed': 10., 'dt': .01,
+            'feedback_enabled': feedback_enabled, 'direct_path': direct_path}
+    assert core.update(straight(), desired, **args) == without_c0.update(straight(), desired, **args)
+    assert core.offset_proportional == 0.
+
+
+def test_direct_path_trial_keeps_its_existing_linear_feedback():
+  core = ModelActionController()
+  for request in [-2., -.01, 0., .01, 2.]:
+    assert correction_for_linear_request(core, request, direct_path=True) == pytest.approx(request)

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-feedback-v21-delayed-feedback')
+      self.assertEqual(record['hypothesis'], 'model-action-curvature-c0-feedback-v22-soft-c0')
       self.assertIs(record['calibration_approved'], False)
       self.assertEqual(record['command'][2:], [0., 0.])
       self.assertEqual(record['status'], controller.diagnostics['status'])

openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py

@@ -196,7 +196,7 @@ def test_actual_controlsd_selection_limiting_publication_and_downstream_can(pipe
   assert controller.core.proportional == pytest.approx(.75*20.*expected_curvature)
   assert controller.core.correction == 0.  # First measurement has no elapsed feedback time.
   assert controls.ford_path.path_angle == pytest.approx((-1 if maneuver else 1)*.0045)
-  assert controls.ford_path.path_offset == pytest.approx((-1 if maneuver else 1)*.02)
+  assert controls.ford_path.path_offset == pytest.approx((-1 if maneuver else 1)*.01)
   assert controller.core.offset_proportional*expected_curvature > 0.
   assert cc.latActive and cc.actuators.curvature == 0.
   assert controller.diagnostics['reference_age'] == pytest.approx(.01 if maneuver else .02)
@@ -412,7 +412,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-feedback-v21-delayed-feedback'
+  assert controls.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v22-soft-c0'
   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.

openpilot/selfdrive/controls/tests/test_ford_model_action_c0_feedback.py

@@ -90,7 +90,8 @@ def test_c0_response_units_and_explicit_gain():
   core.update(straight(), .01, current_curvature=0., speed=5., dt=.01, curvature_scale=1.2)
   # Fitted C0 gain is geometric curvature per metre of command, not road curvature.
   response = .010717679293424373+.018122981795212647/25.
-  assert core.offset_proportional == pytest.approx(.5*.01*1.2/response)
+  assert core.offset_proportional_linear == pytest.approx(.5*.01*1.2/response)
+  assert core.offset_proportional_linear-.0625 < core.offset_proportional < core.offset_proportional_linear
 
 
 @pytest.mark.parametrize('speed', [1., 5., 20., 40.])

openpilot/selfdrive/controls/tests/test_ford_model_action_selection.py

@@ -53,7 +53,7 @@ def test_actual_startup_priority(candidate, observer, fingerprint):
     assert selected.ford_path_controller.core.proportional_gain == C1_PROPORTIONAL_GAIN == .75
     assert selected.ford_path_controller.core.integral_gain == C1_INTEGRAL_GAIN == 1.
     assert selected.ford_path_controller.core.c0_proportional_gain == 1.
-    assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v21-delayed-feedback'
+    assert selected.ford_path_controller.diagnostics['hypothesis'] == 'model-action-curvature-c0-feedback-v22-soft-c0'
   else:
     assert selected.ford_path_controller is None
   assert selected.ford_model_action == candidate

tools/ford_pscm_lab/c0_softening_replay.py

@@ -0,0 +1,115 @@
+"""Replay C0 softening against v21 on frozen recorded motion, not a PSCM model."""
+import argparse
+import hashlib
+import json
+from pathlib import Path
+import subprocess
+from types import SimpleNamespace
+
+import numpy as np
+
+from opendbc.car.vehicle_model import VehicleModel
+from openpilot.selfdrive.controls.lib.ford_model_action import FordModelActionController
+from tools.ford_pscm_lab.model_action_replay import WireCheck, sample, table
+
+
+BASELINE = 'ed9c44f57584b5f033f79db6331e21b9b3e0ad55'
+CONTROLLER = 'openpilot/selfdrive/controls/lib/ford_model_action.py'
+
+
+def replay(source, intake, archive, destination):
+  meta = json.loads((source/'metadata.json').read_text())
+  with np.load(source/'route.npz') as z:
+    r = {k: table(z, k) for k in ('controls', 'cs', 'cc', 'path', 'params', 'pscm')}
+  with np.load(source/'model_paths.npz') as z:
+    paths, path_ns = z['paths'], z['ns']
+  with np.load(intake) as z:
+    delay = z['delay']
+  with np.load(archive) as z:
+    archived = dict(zip(z['names'], z['rows'].T, strict=True))
+  baseline_source = subprocess.check_output(['git', 'show', f'{BASELINE}:{CONTROLLER}'], text=True)
+  namespace = {'__name__': 'v21_ford_controller'}
+  exec(compile(baseline_source, '<v21_ford_controller>', 'exec'), namespace)
+  old, new = namespace['FordModelActionController'](), FordModelActionController()
+  c, cp = r['controls'], meta['car'][0]
+  q = c['t']
+  cs, pa, ps = [sample(r[k], q) for k in ('cs', 'params', 'pscm')]
+  cc, sent = [sample(r[k], q, nearest=True) for k in ('cc', 'path')]
+  mi = np.clip(np.searchsorted(path_ns, c['model_ns']), 0, len(path_ns)-1)
+  exact = path_ns[mi] == c['model_ns']
+  models = [SimpleNamespace(position=SimpleNamespace(x=p[0], y=p[1]), orientation=SimpleNamespace(z=p[2])) for p in paths]
+  delays = sample({'t': delay[:, 0], 'value': delay[:, 3]}, q)['value']
+  delays[q < delay[0, 0]] = 0.
+  params = SimpleNamespace(**{k: cp[k] for k in ('mass', 'wheelbase', 'centerToFront', 'steerRatioRear', 'tireStiffnessFront', 'tireStiffnessRear')},
+                           steerRatio=cp['steer_ratio'], rotationalInertia=0.)
+  vm = VehicleModel(params)
+  wire = WireCheck()
+  names = ['t', 'valid', 'speed', 'desired', 'measured', 'reference', 'target_angle', 'actual_angle',
+           'old_c0', 'new_c0', 'old_p', 'new_p', 'c1', 'c1_p', 'i', 'feedback', 'pressed', 'torque', 'pscm_limit',
+           'recorded_c0', 'recorded_c1']
+  rows = np.zeros((len(q), len(names)))
+  for i, now in enumerate(q):
+    vm.update_params(max(pa['stiffness'][i], .1), max(pa['steer_ratio'][i], .1))
+    scale = vm.get_steer_from_curvature(1., cs['speed'][i], 0.)/(params.steerRatio*params.wheelbase)
+    error = c['desired'][i]-c['measured'][i]
+    if abs(error) > 1e-5:
+      scale = -np.radians(c['desired_angle'][i]-c['actual_angle'][i])/error/(params.steerRatio*params.wheelbase)
+    status = SimpleNamespace(valid=bool(ps['valid'][i] and ps['status_valid'][i]), canMonoTime=round(ps['stamp'][i]*1e9),
+                             limit=int(ps['limit'][i]), lateralState=int(ps['lateral_state'][i]), denied=bool(ps['denied'][i]))
+    args = {'current_curvature': c['measured'][i], 'speed': cs['speed'][i], 'yaw_rate': cs['yaw'][i], 'now': now,
+            'measurement_time': cs['t'][i], 'model_time': c['model_ns'][i]*1e-9, 'reference_time': c['model_ns'][i]*1e-9,
+            'active': bool(cc['active'][i]), 'valid': bool(c['valid'][i] and cs['valid'][i] and cs['can_valid'][i] and pa['valid'][i] and exact[i]),
+            'driver_pressed': bool(cs['pressed'][i]), 'driver_torque': cs['torque'][i], 'pscm_status': status,
+            'curvature_scale': scale, 'lat_delay': delays[i]}
+    before, after = [core.update(models[mi[i]], c['desired'][i], **args) for core in (old, new)]
+    assert before.valid == after.valid
+    assert before.path_angle == after.path_angle and old.core.correction == new.core.correction
+    assert old.core.proportional == new.core.proportional and old.core.feedback_curvature == new.core.feedback_curvature
+    assert old.core.offset_proportional == new.core.offset_proportional_linear
+    assert abs(new.core.offset_proportional) <= abs(old.core.offset_proportional)+1e-12
+    assert new.core.offset_proportional*old.core.offset_proportional >= 0.
+    assert abs(new.core.offset_proportional-old.core.offset_proportional) <= .125000001
+    assert abs(before.path_offset-after.path_offset) <= .130000001
+    assert abs(after.path_offset) <= 5.110000001 and abs(after.path_angle) <= .500000001
+    assert after.curvature == after.curvature_rate == 0.
+    for key in ('feedback_enabled', 'driver_override', 'pscm_limited', 'heading_feedforward', 'offset_overflow', 'feedback_delay'):
+      assert old.diagnostics.get(key) == new.diagnostics.get(key)
+    assert before.path_offset == archived['new_c0'][i] and before.path_angle == archived['new_c1'][i]
+    assert old.core.correction == archived['new_i'][i]
+    if not new.diagnostics.get('feedback_enabled', False):
+      assert new.core.offset_proportional == new.core.proportional == new.core.correction == 0.
+      assert before == after
+    if i % 10 == 0:
+      wire.check(after)
+    rows[i] = [now-meta['t0'], after.valid, cs['speed'][i], c['desired'][i], c['measured'][i], new.core.feedback_curvature,
+               c['desired_angle'][i], c['actual_angle'][i], before.path_offset, after.path_offset,
+               old.core.offset_proportional, new.core.offset_proportional, after.path_angle, new.core.proportional, new.core.correction,
+               new.diagnostics.get('feedback_enabled', False), cs['pressed'][i], cs['torque'][i], ps['limit'][i], sent['c0'][i], sent['c1'][i]]
+  a = dict(zip(names, rows.T, strict=True))
+  valid = a['valid'].astype(bool)
+  dt = np.minimum(np.diff(a['t'], append=a['t'][-1]+.01), .03)
+  paired = valid[1:] & valid[:-1] & (np.diff(a['t']) < .03)
+  metrics = {'cycles': len(q), 'wire_checks': wire.count, 'baseline_matches_archived_v21_exactly': True,
+             'c1_integral_reference_arbitration_equal_every_cycle': True,
+             'P_reduction_p50_p95_p99_max_m': np.quantile(abs(a['old_p'][valid]-a['new_p'][valid]), [.5, .95, .99, 1]).tolist(),
+             'C0_change_p50_p95_p99_max_m': np.quantile(abs(a['old_c0'][valid]-a['new_c0'][valid]), [.5, .95, .99, 1]).tolist(),
+             'variants': {}, 'baseline_commit': BASELINE, 'method': __doc__}
+  for prefix in ('old', 'new'):
+    metrics['variants'][prefix] = {'c0_step_p95_p99_max': np.quantile(abs(np.diff(a[prefix+'_c0'])[paired]), [.95, .99, 1]).tolist(),
+                                  'c0_bound_seconds': float(dt[valid & (abs(a[prefix+'_c0']) >= 5.105)].sum())}
+  metrics['controller_sha256'] = hashlib.sha256(Path(CONTROLLER).read_bytes()).hexdigest()
+  metrics['source_sha256'] = {str(p): hashlib.sha256(p.read_bytes()).hexdigest() for p in [source/'route.npz', source/'model_paths.npz', intake, archive]}
+  destination.mkdir(parents=True, exist_ok=True)
+  np.savez_compressed(destination/'commands.npz', names=names, rows=rows)
+  (destination/'report.json').write_text(json.dumps(metrics, indent=2)+'\n')
+  return {k: v for k, v in metrics.items() if k != 'source_sha256'}
+
+
+if __name__ == '__main__':
+  parser = argparse.ArgumentParser(description=__doc__)
+  parser.add_argument('--source', type=Path, required=True)
+  parser.add_argument('--delay-intake', type=Path, required=True)
+  parser.add_argument('--archive', type=Path, required=True, help='Archived v21 commands.npz for an exact baseline check')
+  parser.add_argument('--output', type=Path, required=True)
+  args = parser.parse_args()
+  print(json.dumps(replay(args.source, args.delay_intake, args.archive, args.output), indent=2))