commaai
/
opendbc
mirror of https://github.com/maweiyuweiwei/opendbc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Toyota: add accel winddown limit (#1481) 

* add rate limits

* add

* oops

* fix

* fix
This commit is contained in:
Shane Smiskol 2024-11-13 17:23:17 -06:00 committed by GitHub
parent 5be5b65d6f
commit f9abb9e473
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
opendbc/car/toyota/carcontroller.py
View File

@ -1,6 +1,6 @@
import math
from opendbc.car import carlog, apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \
make_tester_present_msg, rate_limit, structs, ACCELERATION_DUE_TO_GRAVITY
make_tester_present_msg, rate_limit, structs, ACCELERATION_DUE_TO_GRAVITY, DT_CTRL
from opendbc.car.can_definitions import CanData
from opendbc.car.common.numpy_fast import clip
from opendbc.car.secoc import add_mac, build_sync_mac
@ -15,7 +15,10 @@ LongCtrlState = structs.CarControl.Actuators.LongControlState
SteerControlType = structs.CarParams.SteerControlType
VisualAlert = structs.CarControl.HUDControl.VisualAlert
# The up limit allows the brakes/gas to unwind quickly leaving a stop,
# the down limit matches the rate of ACCEL_NET, reducing PCM compensation windup
ACCEL_WINDUP_LIMIT = 0.5 # m/s^2 / frame
ACCEL_WINDDOWN_LIMIT = -5.0 * DT_CTRL * 3 # m/s^2 / frame
# LKA limits
# EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
@ -171,7 +174,9 @@ class CarController(CarControllerBase):
self.distance_button = 0
# internal PCM gas command can get stuck unwinding from negative accel so we apply a generous rate limit
pcm_accel_cmd = min(actuators.accel, self.prev_accel + ACCEL_WINDUP_LIMIT) if CC.longActive else 0.0
pcm_accel_cmd = actuators.accel
if CC.longActive:
pcm_accel_cmd = rate_limit(pcm_accel_cmd, self.prev_accel, ACCEL_WINDDOWN_LIMIT, ACCEL_WINDUP_LIMIT)
# calculate amount of acceleration PCM should apply to reach target, given pitch
accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY if len(CC.orientationNED) == 3 else 0.0