diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 491c551b1b..c056f7ca3d 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -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 diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index dbc2b33c6b..ff13812398 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -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)) diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index 09bd7ca838..a6ee27a4a3 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -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 diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index cf43b8ef00..6e2869d1c2 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -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) diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index 750fe885e8..52f2aedf98 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -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