body: control with joystick (#24143)

* control with joystick

* slow it down

* always joystick for not car

* clean ups

* not stateful

* move submaster

* only if we aren't in test mode

* update refs

* double ki speed, update ref

* this ref

Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: 7dd71cc63d

selfdrive/car/body/carcontroller.py

@@ -23,10 +23,6 @@ class CarController():
     self.i_balance = 0
     self.d_balance = 0
 
-
-    self.speed_measured = 0.
-    self.speed_desired = 0.
-
     self.torque_r_filtered = 0.
     self.torque_l_filtered = 0.
 
@@ -45,22 +41,18 @@ class CarController():
 
     llk_valid = len(CC.orientationNED) > 0 and len(CC.angularVelocity) > 0
     if CC.enabled and llk_valid:
-
+      # Read these from the joystick
-      # Steer and accel mixin. Speed should be used as a target? (speed should be in m/s! now it is in RPM)
+      # TODO: this isn't acceleration, okay?
-      # Mix steer into torque_diff
+      speed_desired = CC.actuators.accel / 5.
-      # self.steerRatio = 0.5
+      speed_diff_desired = -CC.actuators.steer
-      # 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 / SPEED_FROM_RPM
-      ki_speed = 0.001 / SPEED_FROM_RPM
+      ki_speed = 0.002 / SPEED_FROM_RPM
-      alpha_speed = 1.0
+
+      speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2.
+      speed_error = speed_desired - speed_measured
 
-      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
@@ -77,13 +69,14 @@ class CarController():
       torque = int(np.clip((p_balance*kp_balance + self.i_balance*ki_balance + self.d_balance*kd_balance), -1000, 1000))
 
       # yaw recovery PID
-      kp_turn = 0.1 / SPEED_FROM_RPM
+      kp_turn = 0.95 / SPEED_FROM_RPM
       ki_turn = 0.1 / SPEED_FROM_RPM
 
       speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr)
-      self.i_speed_diff += speed_diff_measured * DT_CTRL
+      speed_diff_error = speed_diff_measured - speed_diff_desired
+      self.i_speed_diff += speed_diff_error * 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))
+      torque_diff = int(np.clip(kp_turn * speed_diff_error + ki_turn * self.i_speed_diff, -100, 100))
 
       # Combine 2 PIDs outputs
       torque_r = torque + torque_diff

selfdrive/car/body/interface.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+import math
 from cereal import car
 from common.realtime import DT_CTRL
 from selfdrive.car import scale_rot_inertia, scale_tire_stiffness, get_safety_config
@@ -15,6 +16,7 @@ class CarInterface(CarInterfaceBase):
     ret.carName = "body"
     ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)]
 
+    ret.minSteerSpeed = -math.inf
     ret.steerRatio = 0.5
     ret.steerRateCost = 0.5
     ret.steerLimitTimer = 1.0

selfdrive/controls/controlsd.py

@@ -72,18 +72,6 @@ class Controls:
     if TICI:
       self.camera_packets.append("wideRoadCameraState")
 
-    params = Params()
-    self.joystick_mode = params.get_bool("JoystickDebugMode")
-    joystick_packet = ['testJoystick'] if self.joystick_mode else []
-
-    self.sm = sm
-    if self.sm is None:
-      ignore = ['driverCameraState', 'managerState'] if SIMULATION else None
-      self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
-                                     'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
-                                     'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet,
-                                     ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan'])
-
     self.can_sock = can_sock
     if can_sock is None:
       can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
@@ -101,6 +89,19 @@ class Controls:
     else:
       self.CI, self.CP = CI, CI.CP
 
+    params = Params()
+    self.joystick_mode = params.get_bool("JoystickDebugMode") or (self.CP.notCar and sm is None)
+    joystick_packet = ['testJoystick'] if self.joystick_mode else []
+
+    self.sm = sm
+    if self.sm is None:
+      ignore = ['driverCameraState', 'managerState'] if SIMULATION else None
+      self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
+                                     'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
+                                     'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet,
+                                     ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan'])
+
+
     # set alternative experiences from parameters
     self.disengage_on_accelerator = params.get_bool("DisengageOnAccelerator")
     self.CP.alternativeExperience = 0

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-65fca83abed98f32993286dc5a66e3e583f06172
+d71efcf28d2216aa53eb4f272514d28c9f96d433