sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Ford: use curvature signal for lateral control (#27091)

Browse Source 
* do carcontroller

Co-authored-by: Cameron Clough <cameronjclough@gmail.com>

* 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 <cameronjclough@gmail.com>
pull/27098/head
Shane Smiskol 2 years ago committed by GitHub
parent cf69653440
commit 2bdce2d92e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 26 additions and 40 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 49
      selfdrive/car/ford/carcontroller.py
  2. 4
      selfdrive/car/ford/fordcan.py
  3. 13
      selfdrive/car/ford/values.py

49
selfdrive/car/ford/carcontroller.py
Unescape View File

@ -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

4
selfdrive/car/ford/fordcan.py
Unescape View File

@ -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]

13
selfdrive/car/ford/values.py
Unescape View File

@ -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

Write Preview
Loading…
Cancel
Save