locationd: body support (#24098)

* body: locationd support

* fewer hacks

* my body is never still

* body on localizer

* minor change

* move wait back to 5 secs

* update refs

* try again

Co-authored-by: nuwandavek <vivekaithal44@gmail.com>
Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: 0b9790dd24

selfdrive/car/body/carcontroller.py

@@ -1,5 +1,6 @@
 import numpy as np
 
+from common.realtime import DT_CTRL
 from selfdrive.car.body import bodycan
 from opendbc.can.packer import CANPacker
 
@@ -23,6 +24,7 @@ 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):
@@ -33,6 +35,9 @@ 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(), []
 

selfdrive/car/body/carstate.py

@@ -14,7 +14,7 @@ class CarState(CarStateBase):
     ret.vEgoRaw = ((ret.wheelSpeeds.fl + ret.wheelSpeeds.fr) / 2.) * self.CP.wheelSpeedFactor
 
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
-    ret.standstill = abs(ret.vEgo) < 1
+    ret.standstill = False
 
     # irrelevant for non-car
     ret.doorOpen = False

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-6c72ebf4ef60b895bde5f0700babf150fd97d523
+7ede67c2798fbe675c078e4d90192efc7e8a3901