Lateral torque-based control with roll on TSS2 corolla and TSSP rav4 (c…

…ommaai#24260) * Initial commit * Fix bugs * Need more torque rate * Cleanup cray cray control * Write nicely * Chiiil * Not relevant for cray cray control * Do some logging * Seems like it has more torque than I thought * Bit more feedforward * Tune change * Retune * Retune * Little more chill * Add coroll * Add corolla * Give craycray a good name * Update to proper logging * D to the PI * Should be in radians * Add d * Start oscillations * Add D term * Only change torque rate limits for new tune * Add d logging * Should be enough * Wrong sign in D * Downtune a little * Needed to prevent faults * Add lqr rav4 to tune * Try derivative again * Data based retune * Data based retune * add friction compensation * Doesnt need too much P with friction comp * remove lqr * Remove kd * Fix tests * fix tests * Too much error * Get roll induced error under 1cm/deg * Too much jitter * Do roll comp * Add ki * Final update * Update refs * Cleanup latcontrol_torque a little more
cfranyota · Jun 22, 2022 · 6bbed9a · 6bbed9a
1 parent d54430a
commit 6bbed9a
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 88 deletions.
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
@@ -18,6 +18,7 @@
 class CarController:
   def __init__(self, dbc_name, CP, VM):
     self.CP = CP
+    self.torque_rate_limits = CarControllerParams(self.CP)
     self.frame = 0
     self.last_steer = 0
     self.alert_active = False
@@ -56,7 +57,7 @@ def update(self, CC, CS):
 
     # 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, CarControllerParams)
+    apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits)
     self.steer_rate_limited = new_steer != apply_steer
 
     # EPS_STATUS->LKA_STATE either goes to 21 or 25 on rising edge of a steering fault and

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
@@ -569,9 +569,10 @@ def state_control(self, CS):
                                                                              lat_plan.psis,
                                                                              lat_plan.curvatures,
                                                                              lat_plan.curvatureRates)
-      actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, params,
-                                                                             self.last_actuators, desired_curvature,
-                                                                             desired_curvature_rate, self.sm['liveLocationKalman'], llk)
+
+      actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM,
+                                                                             params, self.last_actuators, desired_curvature,
+                                                                             desired_curvature_rate, self.sm['liveLocationKalman'])
     else:
       lac_log = log.ControlsState.LateralDebugState.new_message()
       if self.sm.rcv_frame['testJoystick'] > 0:

diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py