diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 28eb382d3b..d30735a84b 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -71,12 +71,17 @@ class CarController: # 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? + apply_angle = actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg torque_sensor_angle = CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg - apply_angle = clip(actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg, -94.9461, 94.9461) + + 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) - apply_steer = clip(apply_steer, - torque_sensor_angle - self.params.ANGLE_DELTA_MAX, - torque_sensor_angle + self.params.ANGLE_DELTA_MAX) + # apply_steer = clip(apply_steer, + # torque_sensor_angle - self.params.ANGLE_DELTA_MAX, + # torque_sensor_angle + self.params.ANGLE_DELTA_MAX) + apply_angle = clip(apply_angle, -94.9461, 94.9461) # 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: @@ -86,7 +91,7 @@ class CarController: apply_steer_req = 1 if not lat_active: - apply_angle = torque_sensor_angle + apply_angle = clip(torque_sensor_angle, -94.9461, 94.9461) apply_steer = 0 apply_steer_req = 0 elif self.steer_rate_counter > MAX_STEER_RATE_FRAMES: @@ -125,7 +130,7 @@ class CarController: 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, + can_sends.append(create_lta_steer_command(self.packer, apply_angle, CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg, CS.out.steeringTorque, apply_steer_req, self.op_params)) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index d8fac4a7b0..54fa1ff096 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -23,7 +23,7 @@ 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 = 0.5 + ANGLE_RATE_MAX = 2.0 # needs to be within +-3 degrees of current angle to avoid windup ANGLE_DELTA_MAX = 3.0