mawei/openpilot-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/openpilot.git synced 2026-08-05 00:05:57 +08:00
Code Issues Packages Projects Releases Wiki Activity

Update disturbance_controller.py

Browse Source
This commit is contained in:
infiniteCable
2025-04-19 13:28:57 +02:00
committed by GitHub
parent 1e9e139919
commit 6e2ff68f91
1 changed files with 3 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/disturbance_controller.py
+3 -1
View File
@@ -24,6 +24,8 @@ PID_KP = 1.0
PID_KI = 0.0 #0.05 # small I to cancel steady wind offset
PID_KF = 0.0
FF_GAIN = 0.2
class DisturbanceController:
"""Wind disturbance compensator using
* 3DoF curvature estimate
@@ -109,7 +111,7 @@ class DisturbanceController:
ay_wind_est = self.Fy_hat / m
# immediate feed forward curvature correction
curv_ff = -ay_wind_est / (v_ego * v_ego + 1e-3)
curv_ff = -ay_wind_est / (v_ego * v_ego + 1e-3) * FF_GAIN
desired_curvature_ff = desired_curvature + curv_ff
# LP/HP separation with adaptive alpha (band pass energy)