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

Toyota: filter pitch for new tune (#1507) 

* Update carcontroller.py

* pitch filter

* 1.5

* clean up

* whoops
This commit is contained in:
Shane Smiskol 2024-11-18 16:23:03 -06:00 committed by GitHub
parent c59aa4a1d5
commit 34af3abb10
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -49,6 +49,8 @@ class CarController(CarControllerBase):
self.steer_rate_counter = 0
self.distance_button = 0
self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL)
self.pcm_accel_compensation = FirstOrderFilter(0, 0.5, DT_CTRL * 3)
# the PCM's reported acceleration request can sometimes mismatch aEgo, close the loop
@ -77,6 +79,9 @@ class CarController(CarControllerBase):
pcm_cancel_cmd = CC.cruiseControl.cancel
lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE
if len(CC.orientationNED) == 3:
self.pitch.update(CC.orientationNED[1])
# *** control msgs ***
can_sends = []
@ -192,7 +197,7 @@ class CarController(CarControllerBase):
self.prev_accel = pcm_accel_cmd
# 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
accel_due_to_pitch = math.sin(self.pitch.x) * ACCELERATION_DUE_TO_GRAVITY
net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch
# For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot and imprecise braking