sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

abstract kf and gear parser as static methods (#1103)

Browse Source 
* Before abstraction, adding speed init from VW as well

* strting to abstract carstate class

* fix bug and update lock?

* revert pipfile change

* another bug

* fix linter

* bug fix

* remove a bunch of diplicated kf code

* better to not have class vars. will abstract __init__ anyway later

* Abstract gear parser static method (#1107)

* abstract common instance vars in carstate init and a generic gear parser static method

* update opendbc (#1105)

* abstract gear parser for chrysler

* update opendbc (#1106)

* abstract gm gear parser too

* remove unnecessary random vars
pull/1112/head
rbiasini 5 years ago committed by GitHub
parent 5469b313c2
commit 5c52be27ca
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
14 changed files with 102 additions and 300 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 50
      selfdrive/car/chrysler/carstate.py
  2. 2
      selfdrive/car/chrysler/interface.py
  3. 37
      selfdrive/car/ford/carstate.py
  4. 43
      selfdrive/car/gm/carstate.py
  5. 4
      selfdrive/car/gm/interface.py
  6. 13
      selfdrive/car/gm/values.py
  7. 51
      selfdrive/car/honda/carstate.py
  8. 41
      selfdrive/car/hyundai/carstate.py
  9. 32
      selfdrive/car/interfaces.py
  10. 2
      selfdrive/car/subaru/carcontroller.py
  11. 33
      selfdrive/car/subaru/carstate.py
  12. 49
      selfdrive/car/toyota/carstate.py
  13. 6
      selfdrive/car/toyota/interface.py
  14. 39
      selfdrive/car/volkswagen/carstate.py

50
selfdrive/car/chrysler/carstate.py
Unescape View File

@ -1,14 +1,7 @@
from cereal import car
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from opendbc.can.can_define import CANDefine
from selfdrive.car.interfaces import CarStateBase
from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD
from common.kalman.simple_kalman import KF1D
GearShifter = car.CarState.GearShifter
def parse_gear_shifter(can_gear):
return {0x1: GearShifter.park, 0x2: GearShifter.reverse, 0x3: GearShifter.neutral,
0x4: GearShifter.drive, 0x5: GearShifter.low}.get(can_gear, GearShifter.unknown)
def get_can_parser(CP): def get_can_parser(CP):
@ -68,26 +61,11 @@ def get_camera_parser(CP):
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
class CarState(): class CarState(CarStateBase):
def __init__(self, CP): def __init__(self, CP):
super().__init__(CP)
self.CP = CP can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.left_blinker_on = 0 self.shifter_values = can_define.dv["GEAR"]['PRNDL']
self.right_blinker_on = 0
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# vEgo kalman filter
dt = 0.01
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
# R = 1e3
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
A=[[1.0, dt], [0.0, 1.0]],
C=[1.0, 0.0],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.0
def update(self, cp, cp_cam): def update(self, cp, cp_cam):
@ -106,28 +84,20 @@ class CarState():
self.brake_pressed = cp.vl["BRAKE_2"]['BRAKE_PRESSED_2'] == 5 # human-only self.brake_pressed = cp.vl["BRAKE_2"]['BRAKE_PRESSED_2'] == 5 # human-only
self.pedal_gas = cp.vl["ACCEL_GAS_134"]['ACCEL_134'] self.pedal_gas = cp.vl["ACCEL_GAS_134"]['ACCEL_134']
self.car_gas = self.pedal_gas
self.esp_disabled = (cp.vl["TRACTION_BUTTON"]['TRACTION_OFF'] == 1) self.esp_disabled = (cp.vl["TRACTION_BUTTON"]['TRACTION_OFF'] == 1)
self.v_wheel_fl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FL'] self.v_wheel_fl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FL']
self.v_wheel_rr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RR'] self.v_wheel_rr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RR']
self.v_wheel_rl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RL'] self.v_wheel_rl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RL']
self.v_wheel_fr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FR'] self.v_wheel_fr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FR']
v_wheel = (cp.vl['SPEED_1']['SPEED_LEFT'] + cp.vl['SPEED_1']['SPEED_RIGHT']) / 2. self.v_ego_raw = (cp.vl['SPEED_1']['SPEED_LEFT'] + cp.vl['SPEED_1']['SPEED_RIGHT']) / 2.
# Kalman filter
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
v_ego_x = self.v_ego_kf.update(v_wheel) self.standstill = not self.v_ego_raw > 0.001
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
self.standstill = not v_wheel > 0.001
self.angle_steers = cp.vl["STEERING"]['STEER_ANGLE'] self.angle_steers = cp.vl["STEERING"]['STEER_ANGLE']
self.angle_steers_rate = cp.vl["STEERING"]['STEERING_RATE'] self.angle_steers_rate = cp.vl["STEERING"]['STEERING_RATE']
self.gear_shifter = parse_gear_shifter(cp.vl['GEAR']['PRNDL']) self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl['GEAR']['PRNDL'], None))
self.main_on = cp.vl["ACC_2"]['ACC_STATUS_2'] == 7 # ACC is green. self.main_on = cp.vl["ACC_2"]['ACC_STATUS_2'] == 7 # ACC is green.
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1 self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2 self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2

2
selfdrive/car/chrysler/interface.py
Unescape View File

@ -135,7 +135,7 @@ class CarInterface(CarInterfaceBase):
ret.gearShifter = self.CS.gear_shifter ret.gearShifter = self.CS.gear_shifter
# gas pedal # gas pedal
ret.gas = self.CS.car_gas ret.gas = self.CS.pedal_gas
ret.gasPressed = self.CS.pedal_gas > 0 ret.gasPressed = self.CS.pedal_gas > 0
# brake pedal # brake pedal

37
selfdrive/car/ford/carstate.py
Unescape View File

@ -1,8 +1,8 @@
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from common.numpy_fast import mean from common.numpy_fast import mean
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.car.interfaces import CarStateBase
from selfdrive.car.ford.values import DBC from selfdrive.car.ford.values import DBC
from common.kalman.simple_kalman import KF1D
WHEEL_RADIUS = 0.33 WHEEL_RADIUS = 0.33
@ -32,26 +32,7 @@ def get_can_parser(CP):
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
class CarState(): class CarState(CarStateBase):
def __init__(self, CP):
self.CP = CP
self.left_blinker_on = 0
self.right_blinker_on = 0
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# vEgo kalman filter
dt = 0.01
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
# R = 1e3
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
A=[[1.0, dt], [0.0, 1.0]],
C=[1.0, 0.0],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.0
def update(self, cp): def update(self, cp):
# update prevs, update must run once per loop # update prevs, update must run once per loop
self.prev_left_blinker_on = self.left_blinker_on self.prev_left_blinker_on = self.left_blinker_on
@ -62,17 +43,9 @@ class CarState():
self.v_wheel_fr = cp.vl["WheelSpeed_CG1"]['WhlRl_W_Meas'] * WHEEL_RADIUS self.v_wheel_fr = cp.vl["WheelSpeed_CG1"]['WhlRl_W_Meas'] * WHEEL_RADIUS
self.v_wheel_rl = cp.vl["WheelSpeed_CG1"]['WhlFr_W_Meas'] * WHEEL_RADIUS self.v_wheel_rl = cp.vl["WheelSpeed_CG1"]['WhlFr_W_Meas'] * WHEEL_RADIUS
self.v_wheel_rr = cp.vl["WheelSpeed_CG1"]['WhlFl_W_Meas'] * WHEEL_RADIUS self.v_wheel_rr = cp.vl["WheelSpeed_CG1"]['WhlFl_W_Meas'] * WHEEL_RADIUS
v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]) self.v_ego_raw = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
# Kalman filter self.standstill = not self.v_ego_raw > 0.001
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
self.standstill = not v_wheel > 0.001
self.angle_steers = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns'] self.angle_steers = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns']
self.v_cruise_pcm = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS self.v_cruise_pcm = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS

43
selfdrive/car/gm/carstate.py
Unescape View File

@ -1,9 +1,10 @@
from cereal import car from cereal import car
from common.numpy_fast import mean from common.numpy_fast import mean
from common.kalman.simple_kalman import KF1D
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from selfdrive.car.gm.values import DBC, CAR, parse_gear_shifter, \ from selfdrive.car.interfaces import CarStateBase
from selfdrive.car.gm.values import DBC, CAR, \
CruiseButtons, is_eps_status_ok, \ CruiseButtons, is_eps_status_ok, \
STEER_THRESHOLD, SUPERCRUISE_CARS STEER_THRESHOLD, SUPERCRUISE_CARS
@ -50,25 +51,11 @@ def get_powertrain_can_parser(CP, canbus):
return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain) return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain)
class CarState(): class CarState(CarStateBase):
def __init__(self, CP, canbus): def __init__(self, CP):
self.CP = CP super().__init__(CP)
# initialize can parser can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = can_define.dv["ECMPRDNL"]["PRNDL"]
self.car_fingerprint = CP.carFingerprint
self.cruise_buttons = CruiseButtons.UNPRESS
self.left_blinker_on = False
self.prev_left_blinker_on = False
self.right_blinker_on = False
self.prev_right_blinker_on = False
# vEgo kalman filter
dt = 0.01
self.v_ego_kf = KF1D(x0=[[0.], [0.]],
A=[[1., dt], [0., 1.]],
C=[1., 0.],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.
def update(self, pt_cp): def update(self, pt_cp):
self.prev_cruise_buttons = self.cruise_buttons self.prev_cruise_buttons = self.cruise_buttons
@ -78,20 +65,12 @@ class CarState():
self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS self.v_wheel_rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS
v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]) self.v_ego_raw = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
self.standstill = self.v_ego_raw < 0.01 self.standstill = self.v_ego_raw < 0.01
self.angle_steers = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle'] self.angle_steers = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
self.gear_shifter = parse_gear_shifter(pt_cp.vl["ECMPRDNL"]['PRNDL']) self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None))
self.user_brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] self.user_brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition']
self.pedal_gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal'] self.pedal_gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal']

4
selfdrive/car/gm/interface.py
Unescape View File

@ -29,7 +29,7 @@ class CarInterface(CarInterfaceBase):
# *** init the major players *** # *** init the major players ***
canbus = CanBus() canbus = CanBus()
self.CS = CarState(CP, canbus) self.CS = CarState(CP)
self.VM = VehicleModel(CP) self.VM = VehicleModel(CP)
self.pt_cp = get_powertrain_can_parser(CP, canbus) self.pt_cp = get_powertrain_can_parser(CP, canbus)
self.ch_cp_dbc_name = DBC[CP.carFingerprint]['chassis'] self.ch_cp_dbc_name = DBC[CP.carFingerprint]['chassis']
@ -237,7 +237,7 @@ class CarInterface(CarInterfaceBase):
be.pressed = self.CS.right_blinker_on be.pressed = self.CS.right_blinker_on
buttonEvents.append(be) buttonEvents.append(be)
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT:
be = car.CarState.ButtonEvent.new_message() be = car.CarState.ButtonEvent.new_message()
be.type = ButtonType.unknown be.type = ButtonType.unknown
if self.CS.cruise_buttons != CruiseButtons.UNPRESS: if self.CS.cruise_buttons != CruiseButtons.UNPRESS:

13
selfdrive/car/gm/values.py
Unescape View File

@ -14,6 +14,7 @@ class CAR:
SUPERCRUISE_CARS = [CAR.CADILLAC_CT6] SUPERCRUISE_CARS = [CAR.CADILLAC_CT6]
class CruiseButtons: class CruiseButtons:
INIT = 0
UNPRESS = 1 UNPRESS = 1
RES_ACCEL = 2 RES_ACCEL = 2
DECEL_SET = 3 DECEL_SET = 3
@ -34,18 +35,6 @@ def is_eps_status_ok(eps_status, car_fingerprint):
valid_eps_status += [0, 1] valid_eps_status += [0, 1]
return eps_status in valid_eps_status return eps_status in valid_eps_status
def parse_gear_shifter(can_gear):
if can_gear == 0:
return car.CarState.GearShifter.park
elif can_gear == 1:
return car.CarState.GearShifter.neutral
elif can_gear == 2:
return car.CarState.GearShifter.drive
elif can_gear == 3:
return car.CarState.GearShifter.reverse
else:
return car.CarState.GearShifter.unknown
FINGERPRINTS = { FINGERPRINTS = {
# Astra BK MY17, ASCM unplugged # Astra BK MY17, ASCM unplugged
CAR.HOLDEN_ASTRA: [{ CAR.HOLDEN_ASTRA: [{

51
selfdrive/car/honda/carstate.py
Unescape View File

@ -1,19 +1,11 @@
from cereal import car
from collections import defaultdict from collections import defaultdict
from common.numpy_fast import interp from common.numpy_fast import interp
from common.kalman.simple_kalman import KF1D
from opendbc.can.can_define import CANDefine from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.car.interfaces import CarStateBase
from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH
GearShifter = car.CarState.GearShifter
def parse_gear_shifter(gear):
return {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
'D': GearShifter.drive, 'S': GearShifter.sport, 'L': GearShifter.low}.get(gear, GearShifter.unknown)
def calc_cruise_offset(offset, speed): def calc_cruise_offset(offset, speed):
# euristic formula so that speed is controlled to ~ 0.3m/s below pid_speed # euristic formula so that speed is controlled to ~ 0.3m/s below pid_speed
# constraints to solve for _K0, _K1, _K2 are: # constraints to solve for _K0, _K1, _K2 are:
@ -188,38 +180,21 @@ def get_cam_can_parser(CP):
bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH and not CP.isPandaBlack else 2 bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH and not CP.isPandaBlack else 2
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam) return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam)
class CarState(): class CarState(CarStateBase):
def __init__(self, CP): def __init__(self, CP):
self.CP = CP super().__init__(CP)
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"] self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
self.steer_status_values = defaultdict(lambda: "UNKNOWN", self.can_define.dv["STEER_STATUS"]["STEER_STATUS"]) self.steer_status_values = defaultdict(lambda: "UNKNOWN", can_define.dv["STEER_STATUS"]["STEER_STATUS"])
self.user_gas, self.user_gas_pressed = 0., 0 self.user_gas, self.user_gas_pressed = 0., 0
self.brake_switch_prev = 0 self.brake_switch_prev = 0
self.brake_switch_ts = 0 self.brake_switch_ts = 0
self.cruise_buttons = 0
self.cruise_setting = 0 self.cruise_setting = 0
self.v_cruise_pcm_prev = 0 self.v_cruise_pcm_prev = 0
self.blinker_on = 0
self.left_blinker_on = 0
self.right_blinker_on = 0
self.cruise_mode = 0 self.cruise_mode = 0
self.stopped = 0 self.stopped = 0
# vEgo kalman filter
dt = 0.01
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
# R = 1e3
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
A=[[1.0, dt], [0.0, 1.0]],
C=[1.0, 0.0],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.0
def update(self, cp, cp_cam): def update(self, cp, cp_cam):
# car params # car params
@ -229,8 +204,6 @@ class CarState():
# update prevs, update must run once per loop # update prevs, update must run once per loop
self.prev_cruise_buttons = self.cruise_buttons self.prev_cruise_buttons = self.cruise_buttons
self.prev_cruise_setting = self.cruise_setting self.prev_cruise_setting = self.cruise_setting
self.prev_blinker_on = self.blinker_on
self.prev_left_blinker_on = self.left_blinker_on self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on self.prev_right_blinker_on = self.right_blinker_on
@ -270,16 +243,10 @@ class CarState():
# blend in transmission speed at low speed, since it has more low speed accuracy # blend in transmission speed at low speed, since it has more low speed accuracy
self.v_weight = interp(v_wheel, v_weight_bp, v_weight_v) self.v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + \ self.v_ego_raw = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + \
self.v_weight * v_wheel self.v_weight * v_wheel
if abs(speed - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
self.v_ego_kf.x = [[speed], [0.0]]
self.v_ego_raw = speed
v_ego_x = self.v_ego_kf.update(speed)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
# this is a hack for the interceptor. This is now only used in the simulation # this is a hack for the interceptor. This is now only used in the simulation
# TODO: Replace tests by toyota so this can go away # TODO: Replace tests by toyota so this can go away
@ -310,7 +277,7 @@ class CarState():
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER']) can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER'])
self.gear_shifter = parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None)) self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None))
self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS'] self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS']
# crv doesn't include cruise control # crv doesn't include cruise control

41
selfdrive/car/hyundai/carstate.py
Unescape View File

@ -1,8 +1,8 @@
from cereal import car from cereal import car
from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from common.kalman.simple_kalman import KF1D
GearShifter = car.CarState.GearShifter GearShifter = car.CarState.GearShifter
@ -124,27 +124,7 @@ def get_camera_parser(CP):
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
class CarState(): class CarState(CarStateBase):
def __init__(self, CP):
self.CP = CP
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# vEgo kalman filter
dt = 0.01
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
# R = 1e3
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
A=[[1.0, dt], [0.0, 1.0]],
C=[1.0, 0.0],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.0
self.left_blinker_on = 0
self.left_blinker_flash = 0
self.right_blinker_on = 0
self.right_blinker_flash = 0
def update(self, cp, cp_cam): def update(self, cp, cp_cam):
# update prevs, update must run once per Loop # update prevs, update must run once per Loop
@ -167,31 +147,22 @@ class CarState():
self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS self.v_wheel_rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4. self.v_ego_raw = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
self.low_speed_lockout = v_wheel < 1.0
# Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default self.low_speed_lockout = self.v_ego_raw < 1.0
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"]) is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS
self.cruise_set_speed = cp.vl["SCC11"]['VSetDis'] * speed_conv self.cruise_set_speed = cp.vl["SCC11"]['VSetDis'] * speed_conv
self.standstill = not v_wheel > 0.1 self.standstill = not self.v_ego_raw > 0.1
self.angle_steers = cp.vl["SAS11"]['SAS_Angle'] self.angle_steers = cp.vl["SAS11"]['SAS_Angle']
self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed'] self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed']
self.yaw_rate = cp.vl["ESP12"]['YAW_RATE'] self.yaw_rate = cp.vl["ESP12"]['YAW_RATE']
self.main_on = True self.main_on = True
self.left_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigLHSw'] self.left_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigLHSw']
self.left_blinker_flash = cp.vl["CGW1"]['CF_Gway_TurnSigLh']
self.right_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigRHSw'] self.right_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigRHSw']
self.right_blinker_flash = cp.vl["CGW1"]['CF_Gway_TurnSigRh']
self.steer_override = abs(cp.vl["MDPS11"]['CR_Mdps_DrvTq']) > STEER_THRESHOLD self.steer_override = abs(cp.vl["MDPS11"]['CR_Mdps_DrvTq']) > STEER_THRESHOLD
self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE
self.steer_error = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail'] self.steer_error = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail']

32
selfdrive/car/interfaces.py
Unescape View File

@ -1,8 +1,12 @@
import os import os
import time import time
from cereal import car from cereal import car
from common.kalman.simple_kalman import KF1D
from common.realtime import DT_CTRL
from selfdrive.car import gen_empty_fingerprint from selfdrive.car import gen_empty_fingerprint
GearShifter = car.CarState.GearShifter
# generic car and radar interfaces # generic car and radar interfaces
class CarInterfaceBase(): class CarInterfaceBase():
@ -42,3 +46,31 @@ class RadarInterfaceBase():
time.sleep(self.radar_ts) # radard runs on RI updates time.sleep(self.radar_ts) # radard runs on RI updates
return ret return ret
class CarStateBase:
def __init__(self, CP):
self.CP = CP
self.car_fingerprint = CP.carFingerprint
self.left_blinker_on = 0
self.right_blinker_on = 0
self.cruise_buttons = 0
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
# R = 1e3
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
A=[[1.0, DT_CTRL], [0.0, 1.0]],
C=[1.0, 0.0],
K=[[0.12287673], [0.29666309]])
def update_speed_kf(self, v_ego_raw):
if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[v_ego_raw], [0.0]]
v_ego_x = self.v_ego_kf.update(v_ego_raw)
return float(v_ego_x[0]), float(v_ego_x[1])
@staticmethod
def parse_gear_shifter(gear):
return {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
'E': GearShifter.eco, 'T': GearShifter.manumatic, 'D': GearShifter.drive,
'S': GearShifter.sport, 'L': GearShifter.low, 'B': GearShifter.brake}.get(gear, GearShifter.unknown)

2
selfdrive/car/subaru/carcontroller.py
Unescape View File

@ -53,7 +53,7 @@ class CarController():
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, P) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, P)
self.steer_rate_limited = new_steer != apply_steer self.steer_rate_limited = new_steer != apply_steer
lkas_enabled = enabled and not CS.steer_not_allowed lkas_enabled = enabled
if not lkas_enabled: if not lkas_enabled:
apply_steer = 0 apply_steer = 0

33
selfdrive/car/subaru/carstate.py
Unescape View File

@ -1,6 +1,6 @@
import copy import copy
from common.kalman.simple_kalman import KF1D
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD
@ -82,29 +82,12 @@ def get_camera_can_parser(CP):
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
class CarState(): class CarState(CarStateBase):
def __init__(self, CP): def __init__(self, CP):
super().__init__(CP)
# initialize can parser # initialize can parser
self.CP = CP
self.car_fingerprint = CP.carFingerprint
self.left_blinker_on = False
self.left_blinker_cnt = 0 self.left_blinker_cnt = 0
self.prev_left_blinker_on = False
self.right_blinker_on = False
self.right_blinker_cnt = 0 self.right_blinker_cnt = 0
self.prev_right_blinker_on = False
self.steer_torque_driver = 0
self.steer_not_allowed = False
self.main_on = False
# vEgo kalman filter
dt = 0.01
self.v_ego_kf = KF1D(x0=[[0.], [0.]],
A=[[1., dt], [0., 1.]],
C=[1., 0.],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.
def update(self, cp, cp_cam): def update(self, cp, cp_cam):
@ -124,16 +107,10 @@ class CarState():
if cp.vl["Dash_State"]['Units'] == 1: if cp.vl["Dash_State"]['Units'] == 1:
self.v_cruise_pcm *= CV.MPH_TO_KPH self.v_cruise_pcm *= CV.MPH_TO_KPH
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4. self.v_ego_raw = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
# Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default # Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
self.standstill = self.v_ego_raw < 0.01 self.standstill = self.v_ego_raw < 0.01
self.prev_left_blinker_on = self.left_blinker_on self.prev_left_blinker_on = self.left_blinker_on

49
selfdrive/car/toyota/carstate.py
Unescape View File

@ -1,17 +1,10 @@
from cereal import car
from common.numpy_fast import mean from common.numpy_fast import mean
from common.kalman.simple_kalman import KF1D
from opendbc.can.can_define import CANDefine from opendbc.can.can_define import CANDefine
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR
GearShifter = car.CarState.GearShifter
def parse_gear_shifter(gear):
return {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral,
'D': GearShifter.drive, 'B': GearShifter.brake}.get(gear, GearShifter.unknown)
def get_can_parser(CP): def get_can_parser(CP):
@ -89,30 +82,14 @@ def get_cam_can_parser(CP):
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
class CarState(): class CarState(CarStateBase):
def __init__(self, CP): def __init__(self, CP):
super().__init__(CP)
self.CP = CP can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = can_define.dv["GEAR_PACKET"]['GEAR']
self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
self.left_blinker_on = 0
self.right_blinker_on = 0
self.angle_offset = 0. self.angle_offset = 0.
self.init_angle_offset = False self.init_angle_offset = False
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# vEgo kalman filter
dt = 0.01
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
# R = 1e3
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
A=[[1.0, dt], [0.0, 1.0]],
C=[1.0, 0.0],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.0
def update(self, cp, cp_cam): def update(self, cp, cp_cam):
# update prevs, update must run once per loop # update prevs, update must run once per loop
self.prev_left_blinker_on = self.left_blinker_on self.prev_left_blinker_on = self.left_blinker_on
@ -127,7 +104,6 @@ class CarState():
self.pedal_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2. self.pedal_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
else: else:
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
self.car_gas = self.pedal_gas
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']
# calc best v_ego estimate, by averaging two opposite corners # calc best v_ego estimate, by averaging two opposite corners
@ -135,17 +111,10 @@ class CarState():
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]) self.v_ego_raw = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
# Kalman filter
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel self.standstill = not self.v_ego_raw > 0.001
v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
self.standstill = not v_wheel > 0.001
if self.CP.carFingerprint in TSS2_CAR: if self.CP.carFingerprint in TSS2_CAR:
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
@ -161,7 +130,7 @@ class CarState():
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE'] self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR']) can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
self.gear_shifter = parse_gear_shifter(self.shifter_values.get(can_gear, None)) self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
if self.CP.carFingerprint == CAR.LEXUS_IS: if self.CP.carFingerprint == CAR.LEXUS_IS:
self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON'] self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON']
else: else:

6
selfdrive/car/toyota/interface.py
Unescape View File

@ -269,9 +269,9 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.66 ret.wheelbase = 2.66
ret.steerRatio = 14.7 ret.steerRatio = 14.7
tire_stiffness_factor = 0.444 # not optimized yet tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
ret.lateralTuning.pid.kf = 0.00006 ret.lateralTuning.pid.kf = 0.00006
ret.steerRateCost = 1. ret.steerRateCost = 1.
ret.centerToFront = ret.wheelbase * 0.44 ret.centerToFront = ret.wheelbase * 0.44
@ -365,7 +365,7 @@ class CarInterface(CarInterfaceBase):
ret.gearShifter = self.CS.gear_shifter ret.gearShifter = self.CS.gear_shifter
# gas pedal # gas pedal
ret.gas = self.CS.car_gas ret.gas = self.CS.pedal_gas
if self.CP.enableGasInterceptor: if self.CP.enableGasInterceptor:
# use interceptor values to disengage on pedal press # use interceptor values to disengage on pedal press
ret.gasPressed = self.CS.pedal_gas > 15 ret.gasPressed = self.CS.pedal_gas > 15

39
selfdrive/car/volkswagen/carstate.py
Unescape View File

@ -1,13 +1,10 @@
import numpy as np import numpy as np
from cereal import car
from common.kalman.simple_kalman import KF1D
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from opendbc.can.can_define import CANDefine from opendbc.can.can_define import CANDefine
from selfdrive.car.volkswagen.values import DBC, BUTTON_STATES, CarControllerParams from selfdrive.car.volkswagen.values import DBC, BUTTON_STATES, CarControllerParams
GEAR = car.CarState.GearShifter
def get_mqb_pt_can_parser(CP, canbus): def get_mqb_pt_can_parser(CP, canbus):
# this function generates lists for signal, messages and initial values # this function generates lists for signal, messages and initial values
signals = [ signals = [
@ -99,31 +96,14 @@ def get_mqb_cam_can_parser(CP, canbus):
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, canbus.cam) return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, canbus.cam)
def parse_gear_shifter(gear):
# Return mapping of gearshift position to selected gear.
return {'P': GEAR.park, 'R': GEAR.reverse, 'N': GEAR.neutral,
'D': GEAR.drive, 'E': GEAR.eco, 'S': GEAR.sport, 'T': GEAR.manumatic}.get(gear, GEAR.unknown)
class CarState(): class CarState(CarStateBase):
def __init__(self, CP, canbus): def __init__(self, CP, canbus):
# initialize can parser super().__init__(CP)
self.CP = CP can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.car_fingerprint = CP.carFingerprint self.shifter_values = can_define.dv["Getriebe_11"]['GE_Fahrstufe']
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["Getriebe_11"]['GE_Fahrstufe']
self.buttonStates = BUTTON_STATES.copy() self.buttonStates = BUTTON_STATES.copy()
# vEgo Kalman filter
dt = 0.01
self.v_ego_kf = KF1D(x0=[[0.], [0.]],
A=[[1., dt], [0., 1.]],
C=[1., 0.],
K=[[0.12287673], [0.29666309]])
self.vEgo = 0.
def update(self, pt_cp): def update(self, pt_cp):
# Update vehicle speed and acceleration from ABS wheel speeds. # Update vehicle speed and acceleration from ABS wheel speeds.
self.wheelSpeedFL = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS self.wheelSpeedFL = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS
@ -132,13 +112,8 @@ class CarState():
self.wheelSpeedRR = pt_cp.vl["ESP_19"]['ESP_HR_Radgeschw_02'] * CV.KPH_TO_MS 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])) self.vEgoRaw = float(np.mean([self.wheelSpeedFL, self.wheelSpeedFR, self.wheelSpeedRL, self.wheelSpeedRR]))
self.vEgo, self.aEgo = self.update_speed_kf(self.vEgoRaw)
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])
self.standstill = self.vEgoRaw < 0.1 self.standstill = self.vEgoRaw < 0.1
# Update steering angle, rate, yaw rate, and driver input torque. VW send # Update steering angle, rate, yaw rate, and driver input torque. VW send
@ -158,7 +133,7 @@ class CarState():
# Update gear and/or clutch position data. # Update gear and/or clutch position data.
can_gear_shifter = int(pt_cp.vl["Getriebe_11"]['GE_Fahrstufe']) can_gear_shifter = int(pt_cp.vl["Getriebe_11"]['GE_Fahrstufe'])
self.gearShifter = parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None)) self.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None))
# Update door and trunk/hatch lid open status. # Update door and trunk/hatch lid open status.
self.doorOpen = any([pt_cp.vl["Gateway_72"]['ZV_FT_offen'], self.doorOpen = any([pt_cp.vl["Gateway_72"]['ZV_FT_offen'],

Write Preview
Loading…
Cancel
Save