Body leash tune (#24105)

* Cleanup variables

* Add clips

* Add good standstill for localizer

* Need abs

* Only on startup

* Cleanup more

* remove violence

* Unused variable

* Update ref

* Not good idea for now

* Update refs again

selfdrive/car/body/carcontroller.py

@@ -3,21 +3,25 @@ import numpy as np
 from common.realtime import DT_CTRL
 from selfdrive.car.body import bodycan
 from opendbc.can.packer import CANPacker
+from selfdrive.car.body.values import SPEED_FROM_RPM
 
 MAX_TORQUE = 500
 MAX_TORQUE_RATE = 50
 MAX_ANGLE_ERROR = 7
+MAX_POS_INTEGRATOR = 0.2 # meters
+MAX_TURN_INTEGRATOR = 0.1 # meters
+
 
 class CarController():
   def __init__(self, dbc_name, CP, VM):
     self.packer = CANPacker(dbc_name)
 
     self.i_speed = 0
+    self.i_speed_diff = 0
 
     self.i_balance = 0
     self.d_balance = 0
 
-    self.i_torque = 0
 
     self.speed_measured = 0.
     self.speed_desired = 0.
@@ -48,49 +52,50 @@ class CarController():
     # torque_r = int(np.clip((CC.actuators.accel*1000) - (CC.actuators.steer*1000) * self.steerRatio, -1000, 1000))
     # torque_l = int(np.clip((CC.actuators.accel*1000) + (CC.actuators.steer*1000) * self.steerRatio, -1000, 1000))
     # ////
+
     # Setpoint speed PID
-    kp_speed = 0.001
-    ki_speed = 0.00001
+    kp_speed = 0.001 / SPEED_FROM_RPM
+    ki_speed = 0.001 / SPEED_FROM_RPM
     alpha_speed = 1.0
 
-    self.speed_measured = (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2.
-    self.speed_desired = (1. - alpha_speed)*self.speed_desired
-    p_speed = (self.speed_desired - self.speed_measured)
-    self.i_speed += ki_speed * p_speed
-    self.i_speed = np.clip(self.i_speed, -0.1, 0.1)
-    set_point = p_speed * kp_speed + self.i_speed
+    self.speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2.
+    self.speed_desired = (1. - alpha_speed) * self.speed_desired
+    speed_error = self.speed_desired - self.speed_measured
+    self.i_speed += speed_error * DT_CTRL
+    self.i_speed = np.clip(self.i_speed, -MAX_POS_INTEGRATOR, MAX_POS_INTEGRATOR)
+    set_point = kp_speed * speed_error + ki_speed * self.i_speed
 
     # Balancing PID
     kp_balance = 1300
     ki_balance = 0
     kd_balance = 280
-    alpha_d_balance = 1.0
 
     # Clip angle error, this is enough to get up from stands
     p_balance = np.clip((-CC.orientationNED[1]) - set_point, np.radians(-MAX_ANGLE_ERROR), np.radians(MAX_ANGLE_ERROR))
     self.i_balance += CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr
-    self.d_balance =  np.clip(((1. - alpha_d_balance) * self.d_balance + alpha_d_balance * -CC.angularVelocity[1]), -1., 1.)
+    self.d_balance = np.clip(-CC.angularVelocity[1], -1., 1.)
     torque = int(np.clip((p_balance*kp_balance + self.i_balance*ki_balance + self.d_balance*kd_balance), -1000, 1000))
 
-    # Positional recovery PID
-    kp_torque = 0.95
-    ki_torque = 0.1
+    # yaw recovery PID
+    kp_turn = 0.1 / SPEED_FROM_RPM
+    ki_turn = 0.1 / SPEED_FROM_RPM
 
-    p_torque = (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr)
-    self.i_torque += (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr)
-    torque_diff = int(np.clip(p_torque*kp_torque + self.i_torque*ki_torque, -100, 100))
+    speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr)
+    self.i_speed_diff += speed_diff_measured * DT_CTRL
+    self.i_speed_diff = np.clip(self.i_speed_diff, -MAX_TURN_INTEGRATOR, MAX_TURN_INTEGRATOR)
+    torque_diff = int(np.clip(kp_turn * speed_diff_measured + ki_turn * self.i_speed_diff, -100, 100))
 
     # Combine 2 PIDs outputs
     torque_r = torque + torque_diff
     torque_l = torque - torque_diff
 
     # Torque rate limits
-    self.torque_r_filtered = np.clip(self.deadband_filter(torque_r, 10) ,
+    self.torque_r_filtered = np.clip(self.deadband_filter(torque_r, 10),
                                      self.torque_r_filtered - MAX_TORQUE_RATE,
-                                     self.torque_r_filtered +  MAX_TORQUE_RATE)
+                                     self.torque_r_filtered + MAX_TORQUE_RATE)
     self.torque_l_filtered = np.clip(self.deadband_filter(torque_l, 10),
                                      self.torque_l_filtered - MAX_TORQUE_RATE,
-                                     self.torque_l_filtered +  MAX_TORQUE_RATE)
+                                     self.torque_l_filtered + MAX_TORQUE_RATE)
     torque_r = int(np.clip(self.torque_r_filtered, -MAX_TORQUE, MAX_TORQUE))
     torque_l = int(np.clip(self.torque_l_filtered, -MAX_TORQUE, MAX_TORQUE))
 

selfdrive/car/body/carstate.py

@@ -3,6 +3,7 @@ from opendbc.can.parser import CANParser
 from selfdrive.car.interfaces import CarStateBase
 from selfdrive.car.body.values import DBC
 
+STARTUP_TICKS = 100
 
 class CarState(CarStateBase):
   def update(self, cp):

selfdrive/car/body/interface.py

@@ -2,6 +2,7 @@
 from cereal import car
 from selfdrive.car import scale_rot_inertia, scale_tire_stiffness, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
+from selfdrive.car.body.values import SPEED_FROM_RPM
 
 class CarInterface(CarInterfaceBase):
   @staticmethod
@@ -20,7 +21,7 @@ class CarInterface(CarInterfaceBase):
 
     ret.mass = 9
     ret.wheelbase = 0.406
-    ret.wheelSpeedFactor = 0.008587
+    ret.wheelSpeedFactor = SPEED_FROM_RPM
     ret.centerToFront = ret.wheelbase * 0.44
 
     ret.radarOffCan = True

selfdrive/car/body/values.py

@@ -5,6 +5,7 @@ from selfdrive.car import dbc_dict
 from selfdrive.car.docs_definitions import CarInfo
 Ecu = car.CarParams.Ecu
 
+SPEED_FROM_RPM = 0.008587
 
 class CarControllerParams:
   ANGLE_DELTA_BP = [0., 5., 15.]

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-abe47ced1758bb6017e61ccf4e8cf19d0b71819e
+f7a6dd296e4747595ec4ef453c58800a88128be7