diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 49d353041adf69..e0b1c3685234b6 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -1,6 +1,5 @@ from cereal import car import cereal.messaging as messaging -import numpy as np from opendbc.can.packer import CANPacker from openpilot.common.numpy_fast import clip, interp from openpilot.common.conversions import Conversions as CV @@ -10,7 +9,6 @@ from openpilot.selfdrive.car.volkswagen import mqbcan, pqcan from openpilot.selfdrive.car.volkswagen.values import CANBUS, CarControllerParams, VolkswagenFlags from openpilot.selfdrive.controls.lib.drive_helpers import VOLKSWAGEN_V_CRUISE_MIN -from openpilot.common.simple_kalman import KF1D, get_kalman_gain VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState @@ -32,21 +30,10 @@ def __init__(self, dbc_name, CP, VM): self.hca_frame_same_torque = 0 self.last_button_frame = 0 self.accel_last = 0 - self.smoothedAccelRate = 0 - - # Define parameters for Kalman Filter - dt = 0.02 # Time step (50hz) - Q = [[0.5, 0.0], [0.0, 0.5]] - R = 10 - A = [[1.0, dt], [0.0, 1.0]] - C = [[1.0, 0.0]] - x0=[[0.0], [0.0]] - K = get_kalman_gain(dt, np.array(A), np.array(C), np.array(Q), R) - self.kf = KF1D(x0=x0, A=A, C=C[0], K=K) - - self.deviationBP = [-3., -0.25, 0., 0.25, 3.] # accel (m/s squared) - self.deviationV = [0., 0.01, 0.03, 0.01, 0.00] # comfort-band (m/s squared) - self.rateLimitBP = [-5., -1.75, -0.25, 0., 0.25, 1.75, 5.] # accel (m/s squared) + + self.deviationBP = [-1.25, -0.25, 0., 0.25, 1.25] # accel (m/s squared) + self.deviationV = [0., 0.075, 0.15, 0.075, 0.] # comfort-band (m/s squared) + self.rateLimitBP = [-3., -1.75, -0.25, 0., 0.25, 1.75, 3.] # accel (m/s squared) self.ratelimitV = [3., 2.50, 0.50, 0.20, 0.50, 1.70, 3.] # jerk-limits (m/s squared) self.longDeviation = 0 self.longRateLimit = 0 @@ -164,9 +151,8 @@ def update(self, CC, CS, now_nanos): accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0 stopping = actuators.longControlState == LongCtrlState.stopping starting = actuators.longControlState == LongCtrlState.pid and (CS.esp_hold_confirmation or CS.out.vEgo < self.CP.vEgoStopping) - self.smoothedAccelRate = self.kf.update(((accel - self.accel_last)*50)) - self.longDeviation = interp(self.smoothedAccelRate[0], self.deviationBP, self.deviationV) - self.longRateLimit = interp(self.smoothedAccelRate[0], self.rateLimitBP, self.ratelimitV) + self.longDeviation = interp(((accel - self.accel_last)*50), self.deviationBP, self.deviationV) + self.longRateLimit = interp(((accel - self.accel_last)*50), self.rateLimitBP, self.ratelimitV) clip(self.longDeviation, self.deviationV[0], self.deviationV[2]) clip(self.longRateLimit, self.ratelimitV[3], self.ratelimitV[0]) self.accel_last = accel