From 868d9b8cd808ad865fea457c5c45c82572f02cf8 Mon Sep 17 00:00:00 2001 From: sshane Date: Sun, 11 Dec 2022 16:26:27 -0800 Subject: [PATCH] clip angle to 3 m/s/s --- selfdrive/car/toyota/carcontroller.py | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index cedd414d1c..83fb354b2e 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -10,6 +10,7 @@ from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, UNSUPPORTED_DSU_CAR from opendbc.can.packer import CANPacker from common.op_params import opParams +import math SteerControlType = car.CarParams.SteerControlType VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -22,7 +23,8 @@ MAX_STEER_RATE_FRAMES = 18 # tx control frames needed before steer can be cut MAX_USER_TORQUE = 500 # EPS ignores commands above this angle and causes PCS faults -MAX_STEER_ANGLE = 94.9461 +MAX_STEER_ANGLE = 94.9461 # deg +MAX_ANGLE_LATERAL_ACCEL = 3.0 # m/s^2 class CarController: @@ -37,6 +39,7 @@ class CarController: self.last_standstill = False self.standstill_req = False self.steer_rate_counter = 0 + self.VM = VM self.packer = CANPacker(dbc_name) self.gas = 0 @@ -89,14 +92,19 @@ class CarController: torque_sensor_angle - self.params.ANGLE_DELTA_MAX, torque_sensor_angle + self.params.ANGLE_DELTA_MAX) + # Clip max angle to acceptable lateral accel limits + v_ego = max(CS.out.vEgo, 5.) + max_steer_angle = abs(MAX_ANGLE_LATERAL_ACCEL / (self.VM.calc_curvature(math.radians(1), v_ego, 0) * v_ego ** 2)) # TODO: roll + apply_angle = clip(apply_angle, -max_steer_angle, max_steer_angle) + # Angular rate limit based on speed apply_angle = apply_std_steer_angle_limits(apply_angle, self.last_angle, CS.out.vEgo, self.params) # Clip to max angle limits - apply_angle = clip(apply_angle, -MAX_STEER_ANGLE, MAX_STEER_ANGLE) + # apply_angle = clip(apply_angle, -MAX_STEER_ANGLE, MAX_STEER_ANGLE) if not lat_active: - apply_angle = clip(torque_sensor_angle, -MAX_STEER_ANGLE, MAX_STEER_ANGLE) + apply_angle = clip(torque_sensor_angle, -max_steer_angle, max_steer_angle) # Count up to MAX_STEER_RATE_FRAMES, at which point we need to cut steer request bit to avoid a steering fault if lat_active and abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE: