body: only control while engaged (#24114)

* body: only control while engaged

* bump panda

* remove that

* bump panda

* update refs

* update refs

panda

@@ -1 +1 @@
-Subproject commit ddf1eb68ba6324022b95103994b11bedb615f608
+Subproject commit ba5cbd5ee1ece245471933a65604f3e7f9520121

selfdrive/car/body/carcontroller.py

@@ -29,7 +29,6 @@ class CarController():
 
     self.torque_r_filtered = 0.
     self.torque_l_filtered = 0.
-    self.wait_counter = int(5. / DT_CTRL)
 
   @staticmethod
   def deadband_filter(torque, deadband):
@@ -40,13 +39,13 @@ class CarController():
     return torque
 
   def update(self, CC, CS):
-    if self.wait_counter > 0:
-      self.wait_counter -= 1
-      return CC.actuators.copy(), []
-    if len(CC.orientationNED) == 0 or len(CC.angularVelocity) == 0:
-      return CC.actuators.copy(), []
 
-    # ///////////////////////////////////////
+    torque_l = 0
+    torque_r = 0
+
+    llk_valid = len(CC.orientationNED) > 0 and len(CC.angularVelocity) > 0
+    if CC.enabled and llk_valid:
+
       # Steer and accel mixin. Speed should be used as a target? (speed should be in m/s! now it is in RPM)
       # Mix steer into torque_diff
       # self.steerRatio = 0.5
@@ -100,8 +99,6 @@ class CarController():
       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))
 
-    # ///////////////////////////////////////
-
     can_sends = []
     can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r, self.frame // 2))
 

selfdrive/car/body/interface.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 from cereal import car
+from common.realtime import DT_CTRL
 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
@@ -41,6 +42,14 @@ class CarInterface(CarInterfaceBase):
 
     ret.canValid = self.cp.can_valid
 
+    # wait for everything to init first
+    if self.frame > int(5. / DT_CTRL):
+      # body always wants to enable
+      ret.init('events', 1)
+      ret.events[0].name = car.CarEvent.EventName.pcmEnable
+      ret.events[0].enable = True
+    self.frame += 1
+
     self.CS.out = ret.as_reader()
     return self.CS.out
 

selfdrive/controls/controlsd.py

@@ -206,6 +206,8 @@ class Controls:
       self.events.add(EventName.pedalPressed)
 
     self.events.add_from_msg(CS.events)
+
+    if not self.CP.notCar:
       self.events.add_from_msg(self.sm['driverMonitoringState'].events)
 
     # Create events for battery, temperature, disk space, and memory

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-594f0e291d2a9720c854f6d0f074b7734ed3c741
+ad17c3c1d11691bc35efe3de78a7cef3daf5dbdf

selfdrive/test/process_replay/test_processes.py

@@ -12,7 +12,7 @@ from tools.lib.logreader import LogReader
 
 
 original_segments = [
-  ("BODY", "d6ac8ebdb47bc549|2022-03-31--13-10-06--4"),        # COMMA.BODY
+  ("BODY", "bd6a637565e91581|2022-04-04--19-14-27--0"),        # COMMA.BODY
   ("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"),     # HYUNDAI.SONATA
   ("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"),     # TOYOTA.PRIUS (INDI)
   ("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"),     # TOYOTA.RAV4  (LQR)
@@ -31,7 +31,7 @@ original_segments = [
 ]
 
 segments = [
-  ("BODY", "d6ac8ebdb47bc549|2022-03-31--13-10-06--4"),
+  ("BODY", "bd6a637565e91581|2022-04-04--19-14-27--0"),
   ("HYUNDAI", "fakedata|2022-01-20--17-49-04--0"),
   ("TOYOTA", "fakedata|2022-01-20--17-50-51--0"),
   ("TOYOTA2", "fakedata|2022-01-20--17-52-36--0"),