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