From fa667f605364821e10ce5903ed57a5831bb16205 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 8 Dec 2022 00:10:32 -0800 Subject: [PATCH] [experiment] apply some rate limiting and anti-windup --- panda | 2 +- selfdrive/car/toyota/carcontroller.py | 71 ++++++++++++++++++--------- selfdrive/car/toyota/toyotacan.py | 4 +- selfdrive/car/toyota/values.py | 6 +++ 4 files changed, 56 insertions(+), 27 deletions(-) diff --git a/panda b/panda index b0d9d0ec96..8f946ce357 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit b0d9d0ec96a1d414c16584d018f8a729914e9c0a +Subproject commit 8f946ce3576a23e57bf3e8933e92d8f32f6acd8e diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 56db35aadd..28eb382d3b 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, from opendbc.can.packer import CANPacker from common.op_params import opParams +SteerControlType = car.CarParams.SteerControlType VisualAlert = car.CarControl.HUDControl.VisualAlert # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long @@ -24,9 +25,10 @@ class CarController: def __init__(self, dbc_name, CP, VM): self.op_params = opParams() self.CP = CP - self.torque_rate_limits = CarControllerParams(self.CP) + self.params = CarControllerParams(self.CP) self.frame = 0 self.last_steer = 0 + self.last_angle = 0 self.alert_active = False self.last_standstill = False self.standstill_req = False @@ -58,11 +60,23 @@ class CarController: interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS) else: interceptor_gas_cmd = 0. - pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) - - # steer torque - new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) - apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits) + pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX) + + # - steer torque + new_steer = int(round(actuators.steer * self.params.STEER_MAX)) + apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params) + + # - steer angle + # 1a. angle command is in terms of the angle signal in the torque sensor message (may or may not have an offset) + # 1b. clip to max angle limits (EPS ignores messages outside of these bounds and causes PCS faults) + # 2. rate limit angle command + # 3. limit max angle error between cmd and actual to reduce EPS integral windup # TODO: can we configure this with a signal? + torque_sensor_angle = CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg + apply_angle = clip(actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg, -94.9461, 94.9461) + apply_angle = clip(apply_angle, self.last_angle - self.params.ANGLE_RATE_MAX, self.last_angle + self.params.ANGLE_RATE_MAX) + apply_steer = clip(apply_steer, + torque_sensor_angle - self.params.ANGLE_DELTA_MAX, + torque_sensor_angle + self.params.ANGLE_DELTA_MAX) # Count up to MAX_STEER_RATE_FRAMES, at which point we need to cut torque to avoid a steering fault if lat_active and abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE: @@ -72,12 +86,18 @@ class CarController: apply_steer_req = 1 if not lat_active: + apply_angle = torque_sensor_angle apply_steer = 0 apply_steer_req = 0 elif self.steer_rate_counter > MAX_STEER_RATE_FRAMES: apply_steer_req = 0 self.steer_rate_counter = 0 + if self.CP.steerControlType == SteerControlType.angle: + apply_steer = 0 + else: + apply_angle = 0 + # TODO: probably can delete this. CS.pcm_acc_status uses a different signal # than CS.cruiseState.enabled. confirm they're not meaningfully different if not CC.enabled and CS.pcm_acc_status: @@ -90,9 +110,6 @@ class CarController: # pcm entered standstill or it's disabled self.standstill_req = False - self.last_steer = apply_steer - self.last_standstill = CS.out.standstill - can_sends = [] # *** control msgs *** @@ -101,20 +118,26 @@ class CarController: # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2; # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages - """ - can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req)) - if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR: - can_sends.append(create_lta_steer_command(self.packer, 0, 0, self.frame // 2)) - """ - - # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda - can_sends.append(create_steer_command(self.packer, 0, 0)) - # On TSS2 cars, the LTA and STEER_TORQUE_SENSOR messages use relative steering angle signals that start - # at 0 degrees, so we need to offset the commanded angle as well. - can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg, - CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg, - CS.out.steeringTorque, - apply_steer_req, self.op_params)) + + if self.CP.steerControlType == SteerControlType.angle: + apply_steer = 0 + # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda + can_sends.append(create_steer_command(self.packer, 0, 0)) + # On TSS2 cars, the LTA and STEER_TORQUE_SENSOR messages use relative steering angle signals that start + # at 0 degrees, so we need to offset the commanded angle as well. + can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg, + CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg, + CS.out.steeringTorque, + apply_steer_req, self.op_params)) + else: + apply_angle = 0 + can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req)) + if TSS2_CAR: + can_sends.append(create_lta_steer_command(self.packer, 0, 0, 0, False, self.op_params)) + + self.last_angle = apply_angle + self.last_steer = apply_steer + self.last_standstill = CS.out.standstill # we can spam can to cancel the system even if we are using lat only control if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd: @@ -165,7 +188,7 @@ class CarController: can_sends.append(make_can_msg(addr, vl, bus)) new_actuators = actuators.copy() - new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX + new_actuators.steer = apply_steer / self.params.STEER_MAX new_actuators.accel = self.accel new_actuators.gas = self.gas diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index a6a0076382..b6e64d0e67 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -15,8 +15,8 @@ def create_steer_command(packer, steer, steer_req): def create_lta_steer_command(packer, apply_steer, steer_angle, driver_torque, steer_req, op_params): """Creates a CAN message for the Toyota LTA Steer Command.""" - percentage = interp(abs(driver_torque), [50, 100], [100, 0]) - apply_steer = interp(percentage, [-10, 100], [steer_angle, apply_steer]) + # percentage = interp(abs(driver_torque), [40, 100], [100, 0]) + # apply_steer = interp(percentage, [-10, 100], [steer_angle, apply_steer]) values = { # seems to actually be 1. Even 1 on 2023 RAV4 2023 (TODO: check from data) "SETME_X1": op_params.get("SETME_X1"), diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 56bb2697c7..d8fac4a7b0 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -21,6 +21,12 @@ class CarControllerParams: STEER_MAX = 1500 STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + # 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 = 0.5 + # needs to be within +-3 degrees of current angle to avoid windup + ANGLE_DELTA_MAX = 3.0 + def __init__(self, CP): if CP.lateralTuning.which == 'torque': self.STEER_DELTA_UP = 15 # 1.0s time to peak torque