From abdcce3ad51a5364b032faa46a2ae8d7ff7af939 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 31 Mar 2022 23:02:47 -0700 Subject: [PATCH] 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 Co-authored-by: Comma Device old-commit-hash: 0b9790dd24d7824746aac42cd06f7fe3a331b3c2 --- selfdrive/car/body/carcontroller.py | 5 +++++ selfdrive/car/body/carstate.py | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index d1dd0535c2..9434eec3ee 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/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(), [] diff --git a/selfdrive/car/body/carstate.py b/selfdrive/car/body/carstate.py index 5f663ae952..9f0ab93fbb 100644 --- a/selfdrive/car/body/carstate.py +++ b/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 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index ebb825c4f8..b75b3d2f21 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -6c72ebf4ef60b895bde5f0700babf150fd97d523 \ No newline at end of file +7ede67c2798fbe675c078e4d90192efc7e8a3901 \ No newline at end of file