diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index 4f7f8c0d0..58054127c 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -43,8 +43,8 @@ class CarController(): self.i_speed = np.clip(self.i_speed, -0.1, 0.1) self.set_point = kp_speed * (self.speed_desired - self.speed_measured) + self.i_speed - angle_err = (-c.CC.orientationNED[1]) - self.set_point - d_new = -c.CC.angularVelocity[1] + angle_err = (-c.orientationNED[1]) - self.set_point + d_new = -c.angularVelocity[1] alpha_d = 1.0 self.d = (1. - alpha_d) * self.d + alpha * d_new self.d = np.clip(self.d, -1., 1.) diff --git a/selfdrive/car/body/carstate.py b/selfdrive/car/body/carstate.py index 3a4e1c1cc..008d44201 100644 --- a/selfdrive/car/body/carstate.py +++ b/selfdrive/car/body/carstate.py @@ -5,9 +5,6 @@ from selfdrive.car.body.values import DBC class CarState(CarStateBase): - def __init__(self, CP): - super().__init__(CP) - def update(self, cp): ret = car.CarState.new_message() @@ -17,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) < 100 + ret.standstill = abs(ret.vEgo) < 1 ret.cruiseState.enabled = True ret.cruiseState.available = True diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index ca6207b17..a07ac0847 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -4,9 +4,6 @@ from selfdrive.car import scale_rot_inertia, scale_tire_stiffness, gen_empty_fin from selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController, CarState): - super().__init__(CP, CarController, CarState) - @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):