From 5c52be27ca8ecef959b1bb7eabcedc0c970d0a8f Mon Sep 17 00:00:00 2001 From: rbiasini Date: Mon, 17 Feb 2020 11:53:53 -0800 Subject: [PATCH] abstract kf and gear parser as static methods (#1103) * 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 --- selfdrive/car/chrysler/carstate.py | 50 ++++++-------------------- selfdrive/car/chrysler/interface.py | 2 +- selfdrive/car/ford/carstate.py | 37 +++---------------- selfdrive/car/gm/carstate.py | 43 ++++++---------------- selfdrive/car/gm/interface.py | 4 +-- selfdrive/car/gm/values.py | 13 +------ selfdrive/car/honda/carstate.py | 51 +++++---------------------- selfdrive/car/hyundai/carstate.py | 41 ++++----------------- selfdrive/car/interfaces.py | 32 +++++++++++++++++ selfdrive/car/subaru/carcontroller.py | 2 +- selfdrive/car/subaru/carstate.py | 33 +++-------------- selfdrive/car/toyota/carstate.py | 49 +++++-------------------- selfdrive/car/toyota/interface.py | 6 ++-- selfdrive/car/volkswagen/carstate.py | 39 ++++---------------- 14 files changed, 102 insertions(+), 300 deletions(-) diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 93b84cf7d9..bbfbdbac43 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -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 diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index f01ad5a090..468acea936 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -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 diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 7484e064ca..978363120a 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -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 diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 8a083cb535..f924c66f17 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -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'] diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index e87c140d71..450fce8538 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -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: diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 468deb01d2..ccde62ea38 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -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: [{ diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 52bea3d7da..8e19565d94 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -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 diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 7ebb8d70d6..e0515b64ec 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -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'] diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index ca1972a167..e98b2681b7 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -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) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 69f573fef8..dce4df810b 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -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 diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 3f0447432c..10500ce6fa 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -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 diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 9ed0fec3f4..35644e5653 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -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: diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 3f9651ce18..ff397e4429 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -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 diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 2db3e818c4..79cd063106 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -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'],