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
Split 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.can_define import CANDefine
from selfdrive.car.interfaces import CarStateBase
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):
@ -68,26 +61,11 @@ def get_camera_parser(CP):
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
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
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = can_define.dv["GEAR"]['PRNDL']
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.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.v_wheel_fl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FL']
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_fr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FR']
v_wheel = (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 = (cp.vl['SPEED_1']['SPEED_LEFT'] + cp.vl['SPEED_1']['SPEED_RIGHT']) / 2.
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.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
self.standstill = not self.v_ego_raw > 0.001
self.angle_steers = cp.vl["STEERING"]['STEER_ANGLE']
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.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
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
# gas pedal
ret.gas = self.CS.car_gas
ret.gas = self.CS.pedal_gas
ret.gasPressed = self.CS.pedal_gas > 0
# brake pedal

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

@ -1,8 +1,8 @@
from opendbc.can.parser import CANParser
from common.numpy_fast import mean
from selfdrive.config import Conversions as CV
from selfdrive.car.interfaces import CarStateBase
from selfdrive.car.ford.values import DBC
from common.kalman.simple_kalman import KF1D
WHEEL_RADIUS = 0.33
@ -32,26 +32,7 @@ def get_can_parser(CP):
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
class CarState():
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
class CarState(CarStateBase):
def update(self, cp):
# update prevs, update must run once per loop
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_rl = cp.vl["WheelSpeed_CG1"]['WhlFr_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])
# 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
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.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)
self.standstill = not self.v_ego_raw > 0.001
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

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

@ -1,9 +1,10 @@
from cereal import car
from common.numpy_fast import mean
from common.kalman.simple_kalman import KF1D
from selfdrive.config import Conversions as CV
from opendbc.can.can_define import CANDefine
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, \
STEER_THRESHOLD, SUPERCRUISE_CARS
@ -50,25 +51,11 @@ def get_powertrain_can_parser(CP, canbus):
return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain)
class CarState():
def __init__(self, CP, canbus):
self.CP = CP
# initialize can parser
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.
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = can_define.dv["ECMPRDNL"]["PRNDL"]
def update(self, pt_cp):
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_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * 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])
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.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)
self.standstill = self.v_ego_raw < 0.01
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.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 ***
canbus = CanBus()
self.CS = CarState(CP, canbus)
self.CS = CarState(CP)
self.VM = VehicleModel(CP)
self.pt_cp = get_powertrain_can_parser(CP, canbus)
self.ch_cp_dbc_name = DBC[CP.carFingerprint]['chassis']
@ -237,7 +237,7 @@ class CarInterface(CarInterfaceBase):
be.pressed = self.CS.right_blinker_on
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.type = ButtonType.unknown
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]
class CruiseButtons:
INIT = 0
UNPRESS = 1
RES_ACCEL = 2
DECEL_SET = 3
@ -34,18 +35,6 @@ def is_eps_status_ok(eps_status, car_fingerprint):
valid_eps_status += [0, 1]
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 = {
# Astra BK MY17, ASCM unplugged
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 common.numpy_fast import interp
from common.kalman.simple_kalman import KF1D
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
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
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):
# euristic formula so that speed is controlled to ~ 0.3m/s below pid_speed
# 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
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam)
class CarState():
class CarState(CarStateBase):
def __init__(self, CP):
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
self.steer_status_values = defaultdict(lambda: "UNKNOWN", self.can_define.dv["STEER_STATUS"]["STEER_STATUS"])
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
self.steer_status_values = defaultdict(lambda: "UNKNOWN", can_define.dv["STEER_STATUS"]["STEER_STATUS"])
self.user_gas, self.user_gas_pressed = 0., 0
self.brake_switch_prev = 0
self.brake_switch_ts = 0
self.cruise_buttons = 0
self.cruise_setting = 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.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):
# car params
@ -229,8 +204,6 @@ class CarState():
# update prevs, update must run once per loop
self.prev_cruise_buttons = self.cruise_buttons
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_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
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
if abs(speed - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
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])
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw)
# 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
@ -310,7 +277,7 @@ class CarState():
self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
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']
# crv doesn't include cruise control

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

@ -1,8 +1,8 @@
from cereal import car
from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
from selfdrive.config import Conversions as CV
from common.kalman.simple_kalman import KF1D
GearShifter = car.CarState.GearShifter
@ -124,27 +124,7 @@ def get_camera_parser(CP):
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
class CarState():
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
class CarState(CarStateBase):
def update(self, cp, cp_cam):
# 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_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
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
self.low_speed_lockout = v_wheel < 1.0
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)
# Kalman filter, even though Hyundai 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_kf.x = [[v_wheel], [0.0]]
self.low_speed_lockout = self.v_ego_raw < 1.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"])
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.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_rate = cp.vl["SAS11"]['SAS_Speed']
self.yaw_rate = cp.vl["ESP12"]['YAW_RATE']
self.main_on = True
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_flash = cp.vl["CGW1"]['CF_Gway_TurnSigRh']
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_error = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail']

32
selfdrive/car/interfaces.py
Unescape View File

@ -1,8 +1,12 @@
import os
import time
from cereal import car
from common.kalman.simple_kalman import KF1D
from common.realtime import DT_CTRL
from selfdrive.car import gen_empty_fingerprint
GearShifter = car.CarState.GearShifter
# generic car and radar interfaces
class CarInterfaceBase():
@ -42,3 +46,31 @@ class RadarInterfaceBase():
time.sleep(self.radar_ts) # radard runs on RI updates
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)
self.steer_rate_limited = new_steer != apply_steer
lkas_enabled = enabled and not CS.steer_not_allowed
lkas_enabled = enabled
if not lkas_enabled:
apply_steer = 0

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

@ -1,6 +1,6 @@
import copy
from common.kalman.simple_kalman import KF1D
from selfdrive.config import Conversions as CV
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
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)
class CarState():
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
# initialize can parser
self.CP = CP
self.car_fingerprint = CP.carFingerprint
self.left_blinker_on = False
self.left_blinker_cnt = 0
self.prev_left_blinker_on = False
self.right_blinker_on = False
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):
@ -124,16 +107,10 @@ class CarState():
if cp.vl["Dash_State"]['Units'] == 1:
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
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, self.a_ego = self.update_speed_kf(self.v_ego_raw)
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.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.kalman.simple_kalman import KF1D
from opendbc.can.can_define import CANDefine
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
from selfdrive.config import Conversions as CV
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):
@ -89,30 +82,14 @@ def get_cam_can_parser(CP):
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
class CarState():
class CarState(CarStateBase):
def __init__(self, CP):
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
self.left_blinker_on = 0
self.right_blinker_on = 0
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = can_define.dv["GEAR_PACKET"]['GEAR']
self.angle_offset = 0.
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):
# update prevs, update must run once per loop
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.
else:
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
self.car_gas = self.pedal_gas
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']
# 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_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
v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
# 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 = 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)
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.standstill = not self.v_ego_raw > 0.001
if self.CP.carFingerprint in TSS2_CAR:
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_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
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:
self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON']
else:

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

@ -269,9 +269,9 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.66
ret.steerRatio = 14.7
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.kf = 0.00006
ret.lateralTuning.pid.kf = 0.00006
ret.steerRateCost = 1.
ret.centerToFront = ret.wheelbase * 0.44
@ -365,7 +365,7 @@ class CarInterface(CarInterfaceBase):
ret.gearShifter = self.CS.gear_shifter
# gas pedal
ret.gas = self.CS.car_gas
ret.gas = self.CS.pedal_gas
if self.CP.enableGasInterceptor:
# use interceptor values to disengage on pedal press
ret.gasPressed = self.CS.pedal_gas > 15

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

@ -1,13 +1,10 @@
import numpy as np
from cereal import car
from common.kalman.simple_kalman import KF1D
from selfdrive.config import Conversions as CV
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
from opendbc.can.can_define import CANDefine
from selfdrive.car.volkswagen.values import DBC, BUTTON_STATES, CarControllerParams
GEAR = car.CarState.GearShifter
def get_mqb_pt_can_parser(CP, canbus):
# this function generates lists for signal, messages and initial values
signals = [
@ -99,31 +96,14 @@ def get_mqb_cam_can_parser(CP, canbus):
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):
# initialize can parser
self.CP = CP
self.car_fingerprint = CP.carFingerprint
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["Getriebe_11"]['GE_Fahrstufe']
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = can_define.dv["Getriebe_11"]['GE_Fahrstufe']
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):
# Update vehicle speed and acceleration from ABS wheel speeds.
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.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
# 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.
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.
self.doorOpen = any([pt_cp.vl["Gateway_72"]['ZV_FT_offen'],

Write Preview
Loading…
Cancel
Save