diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 39b8becebb12a8..394be978682b03 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -9,18 +9,21 @@ def apply_ford_steer_angle_limits(apply_steer, apply_steer_last, vEgo, VM): + apply_angle = apply_steer / VM.sR + apply_angle_last = apply_steer_last / VM.sR + # rate limit - steer_up = apply_steer * apply_steer_last > 0. and abs(apply_steer) > abs(apply_steer_last) - rate_limit = CarControllerParams.STEER_RATE_LIMIT_UP if steer_up else CarControllerParams.STEER_RATE_LIMIT_DOWN + steer_up = apply_angle * apply_angle_last > 0. and abs(apply_angle) > abs(apply_angle_last) + rate_limit = CarControllerParams.ANGLE_RATE_LIMIT_UP if steer_up else CarControllerParams.ANGLE_RATE_LIMIT_DOWN max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points) - apply_steer = clip(apply_steer, (apply_steer_last - max_angle_diff), (apply_steer_last + max_angle_diff)) + apply_angle = clip(apply_angle, (apply_angle_last - max_angle_diff), (apply_angle_last + max_angle_diff)) # absolute limit (0.5 rad after steer ratio) - apply_angle = 4.52182 * math.radians(apply_steer) / VM.sR + apply_angle = math.radians(apply_angle) * 4.52182 apply_angle = clip(apply_angle, -0.5, 0.5235) - apply_steer = math.degrees(apply_angle) * VM.sR / 4.52182 + apply_angle = math.degrees(apply_angle) / 4.52182 - return apply_steer + return apply_angle * VM.sR class CarController: @@ -61,7 +64,7 @@ def update(self, CC, CS): # use LatCtlPath_An_Actl to actuate steering # path angle is the car wheel angle, not the steering wheel angle - path_angle = 4.52182 * math.radians(apply_steer) / self.VM.sR + path_angle = math.radians(apply_steer) * 4.52182 / self.VM.sR # ramp rate: 0=Slow, 1=Medium, 2=Fast, 3=Immediately # TODO: slower ramp speed when driver torque detected diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index aae011ad3dd9c8..48a82b62fe230c 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -19,8 +19,8 @@ class CarControllerParams: # Message: ACCDATA_3 ACC_UI_STEP = 5 - STEER_RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) - STEER_RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) + ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[1.66, 0.26, 0.05]) + ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[1.66, 1.16, 0.133]) class CANBUS: