From 2bdce2d92ec859398aaa749037d1fa58d3b5eec7 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 25 Jan 2023 21:39:46 -0800 Subject: [PATCH] Ford: use curvature signal for lateral control (#27091) * do carcontroller Co-authored-by: Cameron Clough * values done * just send latActive in, cleaner * Update selfdrive/car/ford/values.py * fix a crash * adjust ramp as well Co-authored-by: Cameron Clough --- selfdrive/car/ford/carcontroller.py | 49 ++++++++++------------------- selfdrive/car/ford/fordcan.py | 4 +-- selfdrive/car/ford/values.py | 13 ++++---- 3 files changed, 26 insertions(+), 40 deletions(-) diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 8164a8a970..99a6648ff9 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -1,28 +1,14 @@ import math from cereal import car -from common.numpy_fast import clip, interp +from common.numpy_fast import clip from opendbc.can.packer import CANPacker +from selfdrive.car import apply_std_steer_angle_limits from selfdrive.car.ford.fordcan import create_acc_ui_msg, create_button_msg, create_lat_ctl_msg, create_lka_msg, create_lkas_ui_msg from selfdrive.car.ford.values import CANBUS, CarControllerParams VisualAlert = car.CarControl.HUDControl.VisualAlert -def apply_ford_steer_angle_limits(apply_angle, apply_angle_last, vEgo): - # rate limit - steer_up = apply_angle_last * apply_angle > 0. and abs(apply_angle) > abs(apply_angle_last) - rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN - max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points) - apply_angle = clip(apply_angle, (apply_angle_last - max_angle_diff), (apply_angle_last + max_angle_diff)) - - # absolute limit (LatCtlPath_An_Actl) - apply_path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO - apply_path_angle = clip(apply_path_angle, -0.5, 0.5235) - apply_angle = math.degrees(apply_path_angle) * CarControllerParams.LKAS_STEER_RATIO - - return apply_angle - - class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP @@ -30,7 +16,7 @@ class CarController: self.packer = CANPacker(dbc_name) self.frame = 0 - self.apply_angle_last = 0 + self.apply_curvature_last = 0 self.main_on_last = False self.lkas_enabled_last = False self.steer_alert_last = False @@ -57,34 +43,33 @@ class CarController: can_sends.append(create_button_msg(self.packer, CS.buttons_stock_values, tja_toggle=True)) ### lateral control ### - if CC.latActive: - lca_rq = 1 - apply_angle = apply_ford_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo) - else: - lca_rq = 0 - apply_angle = 0. - # send steering commands at 20Hz if (self.frame % CarControllerParams.STEER_STEP) == 0: - # use LatCtlPath_An_Actl to actuate steering - path_angle = math.radians(apply_angle) / CarControllerParams.LKAS_STEER_RATIO + if CC.latActive: + # apply limits to curvature + apply_curvature = -self.VM.calc_curvature(math.radians(actuators.steeringAngleDeg), CS.out.vEgo, 0.0) + apply_curvature = apply_std_steer_angle_limits(apply_curvature, self.apply_curvature_last, CS.out.vEgo, CarControllerParams) + # clip to signal range + apply_curvature = clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX) + else: + apply_curvature = 0. # set slower ramp type when small steering angle change # 0=Slow, 1=Medium, 2=Fast, 3=Immediately steer_change = abs(CS.out.steeringAngleDeg - actuators.steeringAngleDeg) - if steer_change < 2.0: + if steer_change < 2.5: ramp_type = 0 - elif steer_change < 4.0: + elif steer_change < 5.0: ramp_type = 1 - elif steer_change < 6.0: + elif steer_change < 7.5: ramp_type = 2 else: ramp_type = 3 precision = 1 # 0=Comfortable, 1=Precise (the stock system always uses comfortable) - self.apply_angle_last = apply_angle + self.apply_curvature_last = apply_curvature can_sends.append(create_lka_msg(self.packer)) - can_sends.append(create_lat_ctl_msg(self.packer, lca_rq, ramp_type, precision, 0, path_angle, 0, 0)) + can_sends.append(create_lat_ctl_msg(self.packer, CC.latActive, ramp_type, precision, 0., 0., -apply_curvature, 0.)) ### ui ### send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) @@ -102,7 +87,7 @@ class CarController: self.steer_alert_last = steer_alert new_actuators = actuators.copy() - new_actuators.steeringAngleDeg = self.apply_angle_last + new_actuators.curvature = self.apply_curvature_last self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index c3f6e74651..dcda59bce2 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -16,7 +16,7 @@ def create_lka_msg(packer): return packer.make_can_msg("Lane_Assist_Data1", CANBUS.main, {}) -def create_lat_ctl_msg(packer, lca_rq: int, ramp_type: int, precision: int, path_offset: float, path_angle: float, +def create_lat_ctl_msg(packer, lat_active: bool, ramp_type: int, precision: int, path_offset: float, path_angle: float, curvature: float, curvature_rate: float): """ Creates a CAN message for the Ford TJA/LCA Command. @@ -42,7 +42,7 @@ def create_lat_ctl_msg(packer, lca_rq: int, ramp_type: int, precision: int, path values = { "LatCtlRng_L_Max": 0, # Unknown [0|126] meter "HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1] - "LatCtl_D_Rq": lca_rq, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, + "LatCtl_D_Rq": 1 if lat_active else 0, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, # 3=InterventionRight, 4-7=NotUsed [0|7] "LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] "LatCtlPrecision_D_Rq": precision, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 1348073801..550224e791 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -9,7 +9,7 @@ from selfdrive.car.docs_definitions import CarInfo, Harness from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu -AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points']) +CurvatureLimit = namedtuple('CurvatureLimit', ['speed_points', 'max_angle_diff_points']) class CarControllerParams: @@ -22,12 +22,13 @@ class CarControllerParams: # Message: Steering_Data_FD1, but send twice as fast BUTTONS_STEP = 10 / 2 - LKAS_STEER_RATIO = 2.75 # Approximate ratio between LatCtlPath_An_Actl and steering angle in radians - # TODO: remove this once we understand how the EPS calculates the steering angle better - STEER_DRIVER_ALLOWANCE = 0.8 # Driver intervention threshold in Nm + CURVATURE_MAX = 0.02 # Max curvature for steering command, m^-1 + STEER_DRIVER_ALLOWANCE = 0.8 # Driver intervention threshold, Nm - RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) - RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) + # Curvature rate limits + # TODO: unify field names used by curvature and angle control cars + ANGLE_RATE_LIMIT_UP = CurvatureLimit(speed_points=[5, 15, 25], max_angle_diff_points=[0.005, 0.00056, 0.0002]) + ANGLE_RATE_LIMIT_DOWN = CurvatureLimit(speed_points=[5, 15, 25], max_angle_diff_points=[0.008, 0.00089, 0.00032]) def __init__(self, CP): pass