From 8e3e9141ee74779a0ec49ca24c2ff938d686bb6f Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Thu, 27 Apr 2023 00:43:14 -0700
Subject: [PATCH] Ford safety: curvature error limit (#27981)

* clip Ford requested curvature to current +- delta

* fix bug, allow for more limit

* bump panda to branch

* bump panda

* rename

* rename function

* make a wrapper function (ford uses dynamic up/down limits

* make two functions consistent

* use apply_dist_to_meas_limits

* only above 12 ms

* simplify, clean up

* this isn't used

* https://github.com/commaai/openpilot/pull/27446

* bump panda

* one m/s fudge

* fix current curvature

* also fix panda

* fix panda blocking msgs

* bump panda to fix more blocked msgs

* clip

* bump panda

* lower to 9 ms

* clean up carcontroller

* bump panda

* bump

* bumppanda

* bumppanda

* bumppanda

* split line
---
 panda                               |  2 +-
 selfdrive/car/ford/carcontroller.py | 18 +++++++++++++++---
 selfdrive/car/ford/values.py        |  1 +
 3 files changed, 17 insertions(+), 4 deletions(-)

diff --git a/panda b/panda
index 1e8c5d59c7..c9c3cb38f6 160000
--- a/panda
+++ b/panda
@@ -1 +1 @@
-Subproject commit 1e8c5d59c7aed45aaca580c99197a1248abd0209
+Subproject commit c9c3cb38f6102ed3f72cee9720293be6c581e7ee
diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py
index e5224857ca..6c12a0e6bc 100644
--- a/selfdrive/car/ford/carcontroller.py
+++ b/selfdrive/car/ford/carcontroller.py
@@ -9,6 +9,18 @@ from selfdrive.car.ford.values import CANBUS, CANFD_CARS, CarControllerParams
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
 
+def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw):
+  # Note that we do curvature error limiting after the rate limits since they are just for tuning reasons
+  apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, CarControllerParams)
+
+  # No blending at low speed due to lack of torque wind-up and inaccurate current curvature
+  if v_ego_raw > 9:
+    apply_curvature = clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR,
+                           current_curvature + CarControllerParams.CURVATURE_ERROR)
+
+  return clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX)
+
+
 class CarController:
   def __init__(self, dbc_name, CP, VM):
     self.CP = CP
@@ -46,9 +58,9 @@ class CarController:
     # send steer msg at 20Hz
     if (self.frame % CarControllerParams.STEER_STEP) == 0:
       if CC.latActive:
-        # apply limits to curvature and clip to signal range
-        apply_curvature = apply_std_steer_angle_limits(actuators.curvature, self.apply_curvature_last, CS.out.vEgo, CarControllerParams)
-        apply_curvature = clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX)
+        # apply rate limits, curvature error limit, and clip to signal range
+        current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
+        apply_curvature = apply_ford_curvature_limits(actuators.curvature, self.apply_curvature_last, current_curvature, CS.out.vEgoRaw)
       else:
         apply_curvature = 0.
 
diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py
index ba17f5b54c..9315a97e89 100644
--- a/selfdrive/car/ford/values.py
+++ b/selfdrive/car/ford/values.py
@@ -28,6 +28,7 @@ class CarControllerParams:
   # Worst case, the low speed limits will allow 4.3 m/s^3 up, 4.9 m/s^3 down at 75 mph
   ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.0002, 0.0001])
   ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.000225, 0.00015])
+  CURVATURE_ERROR = 0.002  # ~6 degrees at 10 m/s, ~10 degrees at 35 m/s
 
   ACCEL_MAX = 2.0               # m/s^s max acceleration
   ACCEL_MIN = -3.5              # m/s^s max deceleration