From 8a90671029dedfa6a1a4ff4ed3b9720462fcc68c Mon Sep 17 00:00:00 2001 From: rbiasini Date: Fri, 14 Feb 2020 22:04:59 -0800 Subject: [PATCH] Before abstraction, adding speed init from VW as well (#1101) --- selfdrive/car/volkswagen/carstate.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 59357edbea..2db3e818c4 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -122,6 +122,8 @@ class CarState(): C=[1., 0.], K=[[0.12287673], [0.29666309]]) + self.vEgo = 0. + def update(self, pt_cp): # Update vehicle speed and acceleration from ABS wheel speeds. self.wheelSpeedFL = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS @@ -130,6 +132,10 @@ class CarState(): self.wheelSpeedRR = pt_cp.vl["ESP_19"]['ESP_HR_Radgeschw_02'] * CV.KPH_TO_MS self.vEgoRaw = float(np.mean([self.wheelSpeedFL, self.wheelSpeedFR, self.wheelSpeedRL, self.wheelSpeedRR])) + + if abs(self.vEgoRaw - self.vEgo) > 2.0: # Prevent large accelerations when car starts at non zero speed + self.v_ego_kf.x = [[self.vEgoRaw], [0.0]] + v_ego_x = self.v_ego_kf.update(self.vEgoRaw) self.vEgo = float(v_ego_x[0]) self.aEgo = float(v_ego_x[1])