mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-08-05 08:16:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Car controls: common angle limiting function (#26732)

Browse Source 
* common angle limiting function

* save a line
This commit is contained in:
Shane Smiskol
2022-12-08 16:26:41 -08:00
committed by GitHub
parent 6bea668e35
commit 41667d020a
5 changed files with 27 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/__init__.py
+12 -1
View File
@@ -1,8 +1,9 @@
# functions common among cars
import capnp
from collections import namedtuple
from cereal import car
from common.numpy_fast import clip
from common.numpy_fast import clip, interp
from typing import Dict
# kg of standard extra cargo to count for drive, gas, etc...
@@ -10,6 +11,7 @@ STD_CARGO_KG = 136.
ButtonType = car.CarState.ButtonEvent.Type
EventName = car.CarEvent.EventName
AngleRateLimit = namedtuple('AngleRateLimit', ['speed_bp', 'angle_v'])
def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float:
@@ -111,6 +113,15 @@ def apply_toyota_steer_torque_limits(apply_torque, apply_torque_last, motor_torq
return int(round(float(apply_torque)))
def apply_std_steer_angle_limits(apply_angle, apply_angle_last, v_ego, LIMITS):
# pick angle rate limits based on wind up/down
steer_up = apply_angle_last * apply_angle > 0. and abs(apply_angle) > abs(apply_angle_last)
rate_limits = LIMITS.ANGLE_RATE_LIMIT_UP if steer_up else LIMITS.ANGLE_RATE_LIMIT_DOWN
angle_rate_lim = interp(v_ego, rate_limits.speed_bp, rate_limits.angle_v)
return clip(apply_angle, apply_angle_last - angle_rate_lim, apply_angle_last + angle_rate_lim)
def crc8_pedal(data):
crc = 0xFF # standard init value
poly = 0xD5 # standard crc8: x8+x7+x6+x4+x2+1
selfdrive/car/nissan/carcontroller.py
+4 -11
View File
@@ -1,6 +1,6 @@
from cereal import car
from common.numpy_fast import clip, interp
from opendbc.can.packer import CANPacker
from selfdrive.car import apply_std_steer_angle_limits
from selfdrive.car.nissan import nissancan
from selfdrive.car.nissan.values import CAR, CarControllerParams
@@ -14,7 +14,7 @@ class CarController:
self.frame = 0
self.lkas_max_torque = 0
self.last_angle = 0
self.apply_angle_last = 0
self.packer = CANPacker(dbc_name)
@@ -28,18 +28,11 @@ class CarController:
### STEER ###
lkas_hud_msg = CS.lkas_hud_msg
lkas_hud_info_msg = CS.lkas_hud_info_msg
apply_angle = actuators.steeringAngleDeg
steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0
if CC.latActive:
# windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo, CarControllerParams)
# Max torque from driver before EPS will give up and not apply torque
if not bool(CS.out.steeringPressed):
@@ -57,7 +50,7 @@ class CarController:
apply_angle = CS.out.steeringAngleDeg
self.lkas_max_torque = 0
self.last_angle = apply_angle
self.apply_angle_last = apply_angle
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and pcm_cancel_cmd:
can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg))
selfdrive/car/nissan/values.py
+3 -4
View File
@@ -4,7 +4,7 @@ from enum import Enum
from cereal import car
from panda.python import uds
from selfdrive.car import dbc_dict
from selfdrive.car import AngleRateLimit, dbc_dict
from selfdrive.car.docs_definitions import CarInfo, Harness
from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
@@ -12,9 +12,8 @@ Ecu = car.CarParams.Ecu
class CarControllerParams:
ANGLE_DELTA_BP = [0., 5., 15.]
ANGLE_DELTA_V = [5., .8, .15] # windup limit
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit
ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., .8, .15])
ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., 3.5, 0.4])
LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower
STEER_THRESHOLD = 1.0
selfdrive/car/tesla/carcontroller.py
+5 -9
View File
@@ -1,5 +1,6 @@
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.tesla.teslacan import TeslaCAN
from selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams
@@ -8,7 +9,7 @@ class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.frame = 0
self.last_angle = 0
self.apply_angle_last = 0
self.packer = CANPacker(dbc_name)
self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt'])
self.tesla_can = TeslaCAN(self.packer, self.pt_packer)
@@ -24,20 +25,15 @@ class CarController:
lkas_enabled = CC.latActive and not hands_on_fault
if lkas_enabled:
apply_angle = actuators.steeringAngleDeg
# Angular rate limit based on speed
steer_up = self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle)
rate_limit = CarControllerParams.RATE_LIMIT_UP if steer_up else CarControllerParams.RATE_LIMIT_DOWN
max_angle_diff = interp(CS.out.vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points)
apply_angle = clip(apply_angle, self.last_angle - max_angle_diff, self.last_angle + max_angle_diff)
apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo, CarControllerParams)
# To not fault the EPS
apply_angle = clip(apply_angle, CS.out.steeringAngleDeg - 20, CS.out.steeringAngleDeg + 20)
else:
apply_angle = CS.out.steeringAngleDeg
self.last_angle = apply_angle
self.apply_angle_last = apply_angle
can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, self.frame))
# Longitudinal control (in sync with stock message, about 40Hz)
selfdrive/car/tesla/values.py
+3 -4
View File
@@ -2,14 +2,13 @@ from collections import namedtuple
from typing import Dict, List, Union
from cereal import car
from selfdrive.car import dbc_dict
from selfdrive.car import AngleRateLimit, dbc_dict
from selfdrive.car.docs_definitions import CarInfo
from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = car.CarParams.Ecu
Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points'])
class CAR:
@@ -104,8 +103,8 @@ BUTTONS = [
]
class CarControllerParams:
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])
ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., .8, .15])
ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., 3.5, 0.4])
JERK_LIMIT_MAX = 8
JERK_LIMIT_MIN = -8
ACCEL_TO_SPEED_MULTIPLIER = 3