mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-22 14:32:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

Ford safety: curvature error limit (#27981)

Browse Source 
* 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
This commit is contained in:
Shane Smiskol
2023-04-27 00:43:14 -07:00
committed by GitHub
parent fbd0620442
commit 8e3e9141ee
3 changed files with 17 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
panda
+1 -1
Submodule panda updated: 1e8c5d59c7...c9c3cb38f6
selfdrive/car/ford/carcontroller.py
+15 -3
View File
@@ -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.
selfdrive/car/ford/values.py
+1
View File
@@ -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