diff --git a/panda b/panda index 8f946ce357..5792caca07 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 8f946ce3576a23e57bf3e8933e92d8f32f6acd8e +Subproject commit 5792caca07df2819b82fdadf043afda7b98e6c60 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 174d4f4980..714d70e086 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -77,7 +77,12 @@ class CarController: percentage = interp(abs(CS.out.steeringTorque), [40, 100], [100, 0]) apply_angle = interp(percentage, [-10, 100], [torque_sensor_angle, apply_angle]) - apply_angle = clip(apply_angle, self.last_angle - self.params.ANGLE_RATE_MAX, self.last_angle + self.params.ANGLE_RATE_MAX) + # Angular rate limit based on speed (from TESLA) + steer_up = self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle) + rate_limit = self.params.ANGLE_RATE_LIMIT_UP if steer_up else self.params.ANGLE_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_steer = clip(apply_steer, # torque_sensor_angle - self.params.ANGLE_DELTA_MAX, # torque_sensor_angle + self.params.ANGLE_DELTA_MAX) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 54fa1ff096..08861b5c3f 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -1,4 +1,4 @@ -from collections import defaultdict +from collections import defaultdict, namedtuple from dataclasses import dataclass from enum import Enum, IntFlag from typing import Dict, List, Union @@ -12,6 +12,7 @@ from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQuerie Ecu = car.CarParams.Ecu MIN_ACC_SPEED = 19. * CV.MPH_TO_MS PEDAL_TRANSITION = 10. * CV.MPH_TO_MS +AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points']) class CarControllerParams: @@ -23,7 +24,11 @@ class CarControllerParams: # stock LTA is 0 to 0.05 going straight # and 0.1 to 0.4 when turning (max seen is 0.6303) - ANGLE_RATE_MAX = 2.0 + + ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) + ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) + + # ANGLE_RATE_MAX = 2.0 # needs to be within +-3 degrees of current angle to avoid windup ANGLE_DELTA_MAX = 3.0