[commabody] Support both self-balancing bodies and the wheeled bodies (#29118)

* add wheeled body support

* add WheeledBody param

* make movement less jerky

common/params.cc

@@ -205,6 +205,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"UpdaterTargetBranch", CLEAR_ON_MANAGER_START},
     {"Version", PERSISTENT},
     {"VisionRadarToggle", PERSISTENT},
+    {"WheeledBody", PERSISTENT},
 };
 
 } // namespace

selfdrive/car/body/carcontroller.py

@@ -1,5 +1,6 @@
 import numpy as np
 
+from common.params import Params
 from common.realtime import DT_CTRL
 from opendbc.can.packer import CANPacker
 from selfdrive.car.body import bodycan
@@ -23,10 +24,14 @@ class CarController:
     self.speed_pid = PIDController(0.115, k_i=0.23, rate=1/DT_CTRL)
     self.balance_pid = PIDController(1300, k_i=0, k_d=280, rate=1/DT_CTRL)
     self.turn_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL)
+    self.wheeled_speed_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL)
 
     self.torque_r_filtered = 0.
     self.torque_l_filtered = 0.
 
+    params = Params()
+    self.wheeled_body = params.get("WheeledBody")
+
   @staticmethod
   def deadband_filter(torque, deadband):
     if torque > 0:
@@ -45,19 +50,22 @@ class CarController:
       # Read these from the joystick
       # TODO: this isn't acceleration, okay?
       speed_desired = CC.actuators.accel / 5.
-      speed_diff_desired = -CC.actuators.steer
+      speed_diff_desired = -CC.actuators.steer / 2.
 
       speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2.
       speed_error = speed_desired - speed_measured
 
-      freeze_integrator = ((speed_error < 0 and self.speed_pid.error_integral <= -MAX_POS_INTEGRATOR) or
+      if self.wheeled_body is None:
-                           (speed_error > 0 and self.speed_pid.error_integral >= MAX_POS_INTEGRATOR))
+        freeze_integrator = ((speed_error < 0 and self.speed_pid.error_integral <= -MAX_POS_INTEGRATOR) or 
-      angle_setpoint = self.speed_pid.update(speed_error, freeze_integrator=freeze_integrator)
+                             (speed_error > 0 and self.speed_pid.error_integral >= MAX_POS_INTEGRATOR))
-
+        angle_setpoint = self.speed_pid.update(speed_error, freeze_integrator=freeze_integrator)
-      # Clip angle error, this is enough to get up from stands
+
-      angle_error = np.clip((-CC.orientationNED[1]) - angle_setpoint, -MAX_ANGLE_ERROR, MAX_ANGLE_ERROR)
+        # Clip angle error, this is enough to get up from stands
-      angle_error_rate = np.clip(-CC.angularVelocity[1], -1., 1.)
+        angle_error = np.clip((-CC.orientationNED[1]) - angle_setpoint, -MAX_ANGLE_ERROR, MAX_ANGLE_ERROR)
-      torque = self.balance_pid.update(angle_error, error_rate=angle_error_rate)
+        angle_error_rate = np.clip(-CC.angularVelocity[1], -1., 1.)
+        torque = self.balance_pid.update(angle_error, error_rate=angle_error_rate)
+      else:
+        torque = self.wheeled_speed_pid.update(speed_error, freeze_integrator=False)
 
       speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr)
       turn_error = speed_diff_measured - speed_diff_desired

selfdrive/controls/controlsd.py

@@ -640,7 +640,7 @@ class Controls:
     recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0
 
     # Send a "steering required alert" if saturation count has reached the limit
-    if lac_log.active and not recent_steer_pressed:
+    if lac_log.active and not recent_steer_pressed and not self.CP.notCar:
       if self.CP.lateralTuning.which() == 'torque' and not self.joystick_mode:
         undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2
         turning = abs(lac_log.desiredLateralAccel) > 1.0