mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-06-27 00:52:05 +08:00
Cleanup carstate quoting styles (#21109)
* carstate quoting style: VW
* carstate quoting style: Chrysler
* carstate quoting style: Ford
* carstate quoting style: GM
* carstate quoting style: Honda
* carstate quoting style: Hyundai
* carstate quoting style: Mazda
* carstate quoting style: Nissan
* carstate quoting style: Subaru
* carstate quoting style: Soulless Appliances
* Sorry Honda!
old-commit-hash: 83ca5a0d29
This commit is contained in:
Jason Young
@@ -9,47 +9,47 @@ from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
self.shifter_values = can_define.dv["GEAR"]['PRNDL']
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
self.shifter_values = can_define.dv["GEAR"]["PRNDL"]
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
self.frame = int(cp.vl["EPS_STATUS"]['COUNTER'])
|
||||
self.frame = int(cp.vl["EPS_STATUS"]["COUNTER"])
|
||||
|
||||
ret.doorOpen = any([cp.vl["DOORS"]['DOOR_OPEN_FL'],
|
||||
cp.vl["DOORS"]['DOOR_OPEN_FR'],
|
||||
cp.vl["DOORS"]['DOOR_OPEN_RL'],
|
||||
cp.vl["DOORS"]['DOOR_OPEN_RR']])
|
||||
ret.seatbeltUnlatched = cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_UNLATCHED'] == 1
|
||||
ret.doorOpen = any([cp.vl["DOORS"]["DOOR_OPEN_FL"],
|
||||
cp.vl["DOORS"]["DOOR_OPEN_FR"],
|
||||
cp.vl["DOORS"]["DOOR_OPEN_RL"],
|
||||
cp.vl["DOORS"]["DOOR_OPEN_RR"]])
|
||||
ret.seatbeltUnlatched = cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_UNLATCHED"] == 1
|
||||
|
||||
ret.brakePressed = cp.vl["BRAKE_2"]['BRAKE_PRESSED_2'] == 5 # human-only
|
||||
ret.brakePressed = cp.vl["BRAKE_2"]["BRAKE_PRESSED_2"] == 5 # human-only
|
||||
ret.brake = 0
|
||||
ret.gas = cp.vl["ACCEL_GAS_134"]['ACCEL_134']
|
||||
ret.gas = cp.vl["ACCEL_GAS_134"]["ACCEL_134"]
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
|
||||
ret.espDisabled = (cp.vl["TRACTION_BUTTON"]['TRACTION_OFF'] == 1)
|
||||
ret.espDisabled = (cp.vl["TRACTION_BUTTON"]["TRACTION_OFF"] == 1)
|
||||
|
||||
ret.wheelSpeeds.fl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FL']
|
||||
ret.wheelSpeeds.rr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RR']
|
||||
ret.wheelSpeeds.rl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RL']
|
||||
ret.wheelSpeeds.fr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FR']
|
||||
ret.vEgoRaw = (cp.vl['SPEED_1']['SPEED_LEFT'] + cp.vl['SPEED_1']['SPEED_RIGHT']) / 2.
|
||||
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"]
|
||||
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"]
|
||||
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"]
|
||||
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"]
|
||||
ret.vEgoRaw = (cp.vl["SPEED_1"]["SPEED_LEFT"] + cp.vl["SPEED_1"]["SPEED_RIGHT"]) / 2.
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = not ret.vEgoRaw > 0.001
|
||||
|
||||
ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
|
||||
ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
|
||||
ret.steeringAngleDeg = cp.vl["STEERING"]['STEER_ANGLE']
|
||||
ret.steeringRateDeg = cp.vl["STEERING"]['STEERING_RATE']
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl['GEAR']['PRNDL'], None))
|
||||
ret.leftBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1
|
||||
ret.rightBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2
|
||||
ret.steeringAngleDeg = cp.vl["STEERING"]["STEER_ANGLE"]
|
||||
ret.steeringRateDeg = cp.vl["STEERING"]["STEERING_RATE"]
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl["GEAR"]["PRNDL"], None))
|
||||
|
||||
ret.cruiseState.enabled = cp.vl["ACC_2"]['ACC_STATUS_2'] == 7 # ACC is green.
|
||||
ret.cruiseState.enabled = cp.vl["ACC_2"]["ACC_STATUS_2"] == 7 # ACC is green.
|
||||
ret.cruiseState.available = ret.cruiseState.enabled # FIXME: for now same as enabled
|
||||
ret.cruiseState.speed = cp.vl["DASHBOARD"]['ACC_SPEED_CONFIG_KPH'] * CV.KPH_TO_MS
|
||||
ret.cruiseState.speed = cp.vl["DASHBOARD"]["ACC_SPEED_CONFIG_KPH"] * CV.KPH_TO_MS
|
||||
# CRUISE_STATE is a three bit msg, 0 is off, 1 and 2 are Non-ACC mode, 3 and 4 are ACC mode, find if there are other states too
|
||||
ret.cruiseState.nonAdaptive = cp.vl["DASHBOARD"]['CRUISE_STATE'] in [1, 2]
|
||||
ret.cruiseState.nonAdaptive = cp.vl["DASHBOARD"]["CRUISE_STATE"] in [1, 2]
|
||||
|
||||
ret.steeringTorque = cp.vl["EPS_STATUS"]["TORQUE_DRIVER"]
|
||||
ret.steeringTorqueEps = cp.vl["EPS_STATUS"]["TORQUE_MOTOR"]
|
||||
@@ -57,15 +57,15 @@ class CarState(CarStateBase):
|
||||
steer_state = cp.vl["EPS_STATUS"]["LKAS_STATE"]
|
||||
ret.steerError = steer_state == 4 or (steer_state == 0 and ret.vEgo > self.CP.minSteerSpeed)
|
||||
|
||||
ret.genericToggle = bool(cp.vl["STEERING_LEVERS"]['HIGH_BEAM_FLASH'])
|
||||
ret.genericToggle = bool(cp.vl["STEERING_LEVERS"]["HIGH_BEAM_FLASH"])
|
||||
|
||||
if self.CP.enableBsm:
|
||||
ret.leftBlindspot = cp.vl["BLIND_SPOT_WARNINGS"]['BLIND_SPOT_LEFT'] == 1
|
||||
ret.rightBlindspot = cp.vl["BLIND_SPOT_WARNINGS"]['BLIND_SPOT_RIGHT'] == 1
|
||||
ret.leftBlindspot = cp.vl["BLIND_SPOT_WARNINGS"]["BLIND_SPOT_LEFT"] == 1
|
||||
ret.rightBlindspot = cp.vl["BLIND_SPOT_WARNINGS"]["BLIND_SPOT_RIGHT"] == 1
|
||||
|
||||
self.lkas_counter = cp_cam.vl["LKAS_COMMAND"]['COUNTER']
|
||||
self.lkas_car_model = cp_cam.vl["LKAS_HUD"]['CAR_MODEL']
|
||||
self.lkas_status_ok = cp_cam.vl["LKAS_HEARTBIT"]['LKAS_STATUS_OK']
|
||||
self.lkas_counter = cp_cam.vl["LKAS_COMMAND"]["COUNTER"]
|
||||
self.lkas_car_model = cp_cam.vl["LKAS_HUD"]["CAR_MODEL"]
|
||||
self.lkas_status_ok = cp_cam.vl["LKAS_HEARTBIT"]["LKAS_STATUS_OK"]
|
||||
|
||||
return ret
|
||||
|
||||
@@ -125,7 +125,7 @@ class CarState(CarStateBase):
|
||||
]
|
||||
checks += [("BLIND_SPOT_WARNINGS", 2)]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser(CP):
|
||||
@@ -141,4 +141,4 @@ class CarState(CarStateBase):
|
||||
("LKAS_HUD", 4),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
|
||||
|
||||
@@ -10,25 +10,25 @@ WHEEL_RADIUS = 0.33
|
||||
class CarState(CarStateBase):
|
||||
def update(self, cp):
|
||||
ret = car.CarState.new_message()
|
||||
ret.wheelSpeeds.rr = cp.vl["WheelSpeed_CG1"]['WhlRr_W_Meas'] * WHEEL_RADIUS
|
||||
ret.wheelSpeeds.rl = cp.vl["WheelSpeed_CG1"]['WhlRl_W_Meas'] * WHEEL_RADIUS
|
||||
ret.wheelSpeeds.fr = cp.vl["WheelSpeed_CG1"]['WhlFr_W_Meas'] * WHEEL_RADIUS
|
||||
ret.wheelSpeeds.fl = cp.vl["WheelSpeed_CG1"]['WhlFl_W_Meas'] * WHEEL_RADIUS
|
||||
ret.wheelSpeeds.rr = cp.vl["WheelSpeed_CG1"]["WhlRr_W_Meas"] * WHEEL_RADIUS
|
||||
ret.wheelSpeeds.rl = cp.vl["WheelSpeed_CG1"]["WhlRl_W_Meas"] * WHEEL_RADIUS
|
||||
ret.wheelSpeeds.fr = cp.vl["WheelSpeed_CG1"]["WhlFr_W_Meas"] * WHEEL_RADIUS
|
||||
ret.wheelSpeeds.fl = cp.vl["WheelSpeed_CG1"]["WhlFl_W_Meas"] * WHEEL_RADIUS
|
||||
ret.vEgoRaw = mean([ret.wheelSpeeds.rr, ret.wheelSpeeds.rl, ret.wheelSpeeds.fr, ret.wheelSpeeds.fl])
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = not ret.vEgoRaw > 0.001
|
||||
ret.steeringAngleDeg = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns']
|
||||
ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]['LaHandsOff_B_Actl']
|
||||
ret.steerError = cp.vl["Lane_Keep_Assist_Status"]['LaActDeny_B_Actl'] == 1
|
||||
ret.cruiseState.speed = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS
|
||||
ret.cruiseState.enabled = not (cp.vl["Cruise_Status"]['Cruise_State'] in [0, 3])
|
||||
ret.cruiseState.available = cp.vl["Cruise_Status"]['Cruise_State'] != 0
|
||||
ret.gas = cp.vl["EngineData_14"]['ApedPosScal_Pc_Actl'] / 100.
|
||||
ret.steeringAngleDeg = cp.vl["Steering_Wheel_Data_CG1"]["SteWhlRelInit_An_Sns"]
|
||||
ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]["LaHandsOff_B_Actl"]
|
||||
ret.steerError = cp.vl["Lane_Keep_Assist_Status"]["LaActDeny_B_Actl"] == 1
|
||||
ret.cruiseState.speed = cp.vl["Cruise_Status"]["Set_Speed"] * CV.MPH_TO_MS
|
||||
ret.cruiseState.enabled = not (cp.vl["Cruise_Status"]["Cruise_State"] in [0, 3])
|
||||
ret.cruiseState.available = cp.vl["Cruise_Status"]["Cruise_State"] != 0
|
||||
ret.gas = cp.vl["EngineData_14"]["ApedPosScal_Pc_Actl"] / 100.
|
||||
ret.gasPressed = ret.gas > 1e-6
|
||||
ret.brakePressed = bool(cp.vl["Cruise_Status"]["Brake_Drv_Appl"])
|
||||
ret.genericToggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"])
|
||||
# TODO: we also need raw driver torque, needed for Assisted Lane Change
|
||||
self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]['LaActAvail_D_Actl']
|
||||
self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]["LaActAvail_D_Actl"]
|
||||
|
||||
return ret
|
||||
|
||||
@@ -51,4 +51,4 @@ class CarState(CarStateBase):
|
||||
("Brake_Drv_Appl", "Cruise_Status", 0.),
|
||||
]
|
||||
checks = []
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0, enforce_checks=False)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0, enforce_checks=False)
|
||||
|
||||
@@ -11,62 +11,62 @@ from selfdrive.car.gm.values import DBC, CAR, AccState, CanBus, \
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
self.shifter_values = can_define.dv["ECMPRDNL"]["PRNDL"]
|
||||
|
||||
def update(self, pt_cp):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
self.prev_cruise_buttons = self.cruise_buttons
|
||||
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons']
|
||||
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
|
||||
|
||||
ret.wheelSpeeds.fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fl = pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"] * CV.KPH_TO_MS
|
||||
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = ret.vEgoRaw < 0.01
|
||||
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None))
|
||||
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] / 0xd0
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]["PRNDL"], None))
|
||||
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0
|
||||
# Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed.
|
||||
if ret.brake < 10/0xd0:
|
||||
ret.brake = 0.
|
||||
|
||||
ret.gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal'] / 254.
|
||||
ret.gas = pt_cp.vl["AcceleratorPedal"]["AcceleratorPedal"] / 254.
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
|
||||
ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
|
||||
ret.steeringRateDeg = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelRate']
|
||||
ret.steeringTorque = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
|
||||
ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]['LKATorqueDelivered']
|
||||
ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelAngle"]
|
||||
ret.steeringRateDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelRate"]
|
||||
ret.steeringTorque = pt_cp.vl["PSCMStatus"]["LKADriverAppldTrq"]
|
||||
ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]["LKATorqueDelivered"]
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
|
||||
|
||||
# 0 inactive, 1 active, 2 temporarily limited, 3 failed
|
||||
self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus']
|
||||
self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"]
|
||||
ret.steerWarning = self.lkas_status not in [0, 1]
|
||||
|
||||
# 1 - open, 0 - closed
|
||||
ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 1 or
|
||||
pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 1 or
|
||||
pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 1 or
|
||||
pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 1)
|
||||
ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or
|
||||
pt_cp.vl["BCMDoorBeltStatus"]["FrontRightDoor"] == 1 or
|
||||
pt_cp.vl["BCMDoorBeltStatus"]["RearLeftDoor"] == 1 or
|
||||
pt_cp.vl["BCMDoorBeltStatus"]["RearRightDoor"] == 1)
|
||||
|
||||
# 1 - latched
|
||||
ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 0
|
||||
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1
|
||||
ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2
|
||||
ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0
|
||||
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
|
||||
ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
|
||||
|
||||
self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed']
|
||||
ret.cruiseState.available = bool(pt_cp.vl["ECMEngineStatus"]['CruiseMainOn'])
|
||||
ret.espDisabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1
|
||||
self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState']
|
||||
self.park_brake = pt_cp.vl["EPBStatus"]["EPBClosed"]
|
||||
ret.cruiseState.available = bool(pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"])
|
||||
ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
|
||||
self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]["CruiseState"]
|
||||
|
||||
ret.brakePressed = ret.brake > 1e-5
|
||||
# Regen braking is braking
|
||||
if self.car_fingerprint == CAR.VOLT:
|
||||
ret.brakePressed = ret.brakePressed or bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
|
||||
ret.brakePressed = ret.brakePressed or bool(pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"])
|
||||
|
||||
ret.cruiseState.enabled = self.pcm_acc_status != AccState.OFF
|
||||
ret.cruiseState.standstill = self.pcm_acc_status == AccState.STANDSTILL
|
||||
@@ -129,4 +129,4 @@ class CarState(CarStateBase):
|
||||
("EBCMRegenPaddle", 50),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, CanBus.POWERTRAIN)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.POWERTRAIN)
|
||||
|
||||
@@ -195,7 +195,7 @@ def get_can_signals(CP):
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
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"])
|
||||
|
||||
@@ -220,114 +220,114 @@ class CarState(CarStateBase):
|
||||
# ******************* parse out can *******************
|
||||
# TODO: find wheels moving bit in dbc
|
||||
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G):
|
||||
ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
|
||||
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'])
|
||||
ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 0.1
|
||||
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"])
|
||||
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
|
||||
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN'])
|
||||
ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 0.1
|
||||
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"])
|
||||
elif self.CP.carFingerprint == CAR.HRV:
|
||||
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN'])
|
||||
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"])
|
||||
else:
|
||||
ret.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING']
|
||||
ret.doorOpen = any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'],
|
||||
cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']])
|
||||
ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] or not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED'])
|
||||
ret.standstill = not cp.vl["STANDSTILL"]["WHEELS_MOVING"]
|
||||
ret.doorOpen = any([cp.vl["DOORS_STATUS"]["DOOR_OPEN_FL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_FR"],
|
||||
cp.vl["DOORS_STATUS"]["DOOR_OPEN_RL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_RR"]])
|
||||
ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LAMP"] or not cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LATCHED"])
|
||||
|
||||
steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]['STEER_STATUS']]
|
||||
ret.steerError = steer_status not in ['NORMAL', 'NO_TORQUE_ALERT_1', 'NO_TORQUE_ALERT_2', 'LOW_SPEED_LOCKOUT', 'TMP_FAULT']
|
||||
steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]["STEER_STATUS"]]
|
||||
ret.steerError = steer_status not in ["NORMAL", "NO_TORQUE_ALERT_1", "NO_TORQUE_ALERT_2", "LOW_SPEED_LOCKOUT", "TMP_FAULT"]
|
||||
# NO_TORQUE_ALERT_2 can be caused by bump OR steering nudge from driver
|
||||
self.steer_not_allowed = steer_status not in ['NORMAL', 'NO_TORQUE_ALERT_2']
|
||||
self.steer_not_allowed = steer_status not in ["NORMAL", "NO_TORQUE_ALERT_2"]
|
||||
# LOW_SPEED_LOCKOUT is not worth a warning
|
||||
ret.steerWarning = steer_status not in ['NORMAL', 'LOW_SPEED_LOCKOUT', 'NO_TORQUE_ALERT_2']
|
||||
ret.steerWarning = steer_status not in ["NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2"]
|
||||
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
self.brake_error = 0
|
||||
else:
|
||||
self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl["STANDSTILL"]['BRAKE_ERROR_2']
|
||||
ret.espDisabled = cp.vl["VSA_STATUS"]['ESP_DISABLED'] != 0
|
||||
self.brake_error = cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"]
|
||||
ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0
|
||||
|
||||
speed_factor = SPEED_FACTOR[self.CP.carFingerprint]
|
||||
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor
|
||||
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor
|
||||
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor
|
||||
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor
|
||||
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"] * CV.KPH_TO_MS * speed_factor
|
||||
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"] * CV.KPH_TO_MS * speed_factor
|
||||
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"] * CV.KPH_TO_MS * speed_factor
|
||||
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"] * CV.KPH_TO_MS * speed_factor
|
||||
v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr)/4.
|
||||
|
||||
# blend in transmission speed at low speed, since it has more low speed accuracy
|
||||
v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
|
||||
ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + v_weight * v_wheel
|
||||
ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] * CV.KPH_TO_MS * speed_factor + v_weight * v_wheel
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
|
||||
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
|
||||
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
|
||||
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE"]
|
||||
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE_RATE"]
|
||||
|
||||
self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
|
||||
self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
|
||||
self.cruise_setting = cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"]
|
||||
self.cruise_buttons = cp.vl["SCM_BUTTONS"]["CRUISE_BUTTONS"]
|
||||
|
||||
ret.leftBlinker = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] != 0
|
||||
ret.rightBlinker = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER'] != 0
|
||||
self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE']
|
||||
ret.leftBlinker = cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"] != 0
|
||||
ret.rightBlinker = cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"] != 0
|
||||
self.brake_hold = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"]
|
||||
|
||||
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH,
|
||||
CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G):
|
||||
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
|
||||
main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON']
|
||||
self.park_brake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
|
||||
main_on = cp.vl["SCM_FEEDBACK"]["MAIN_ON"]
|
||||
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0
|
||||
main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
|
||||
self.park_brake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
|
||||
main_on = cp.vl["SCM_BUTTONS"]["MAIN_ON"]
|
||||
else:
|
||||
self.park_brake = 0 # TODO
|
||||
main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON']
|
||||
main_on = cp.vl["SCM_BUTTONS"]["MAIN_ON"]
|
||||
|
||||
gear = int(cp.vl["GEARBOX"]['GEAR_SHIFTER'])
|
||||
gear = int(cp.vl["GEARBOX"]["GEAR_SHIFTER"])
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, 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
|
||||
if self.CP.carFingerprint in (CAR.CRV, CAR.CRV_EU, CAR.HRV, CAR.ODYSSEY, CAR.ACURA_RDX, CAR.RIDGELINE, CAR.PILOT_2019, CAR.ODYSSEY_CHN):
|
||||
ret.gas = self.pedal_gas / 256.
|
||||
else:
|
||||
ret.gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS'] / 256.
|
||||
ret.gas = cp.vl["GAS_PEDAL_2"]["CAR_GAS"] / 256.
|
||||
|
||||
# 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
|
||||
if self.CP.enableGasInterceptor:
|
||||
self.user_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
|
||||
self.user_gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
|
||||
self.user_gas_pressed = self.user_gas > 1e-5 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
|
||||
ret.gasPressed = self.user_gas_pressed
|
||||
else:
|
||||
ret.gasPressed = self.pedal_gas > 1e-5
|
||||
|
||||
ret.steeringTorque = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']
|
||||
ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]['MOTOR_TORQUE']
|
||||
ret.steeringTorque = cp.vl["STEER_STATUS"]["STEER_TORQUE_SENSOR"]
|
||||
ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]["MOTOR_TORQUE"]
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.CP.carFingerprint]
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
self.cruise_mode = cp.vl["ACC_HUD"]['CRUISE_CONTROL_LABEL']
|
||||
ret.cruiseState.standstill = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252.
|
||||
self.cruise_mode = cp.vl["ACC_HUD"]["CRUISE_CONTROL_LABEL"]
|
||||
ret.cruiseState.standstill = cp.vl["ACC_HUD"]["CRUISE_SPEED"] == 252.
|
||||
ret.cruiseState.speedOffset = calc_cruise_offset(0, ret.vEgo)
|
||||
# On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
|
||||
ret.cruiseState.speed = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]['CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED'] * CV.KPH_TO_MS
|
||||
ret.cruiseState.speed = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]["CRUISE_SPEED"] > 160.0 else cp.vl["ACC_HUD"]["CRUISE_SPEED"] * CV.KPH_TO_MS
|
||||
self.v_cruise_pcm_prev = ret.cruiseState.speed
|
||||
else:
|
||||
ret.cruiseState.speedOffset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], ret.vEgo)
|
||||
ret.cruiseState.speed = cp.vl["CRUISE"]['CRUISE_SPEED_PCM'] * CV.KPH_TO_MS
|
||||
ret.cruiseState.speedOffset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]["CRUISE_SPEED_OFFSET"], ret.vEgo)
|
||||
ret.cruiseState.speed = cp.vl["CRUISE"]["CRUISE_SPEED_PCM"] * CV.KPH_TO_MS
|
||||
|
||||
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != 0
|
||||
self.brake_switch = cp.vl["POWERTRAIN_DATA"]["BRAKE_SWITCH"] != 0
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
|
||||
ret.brakePressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] != 0
|
||||
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
|
||||
else:
|
||||
# brake switch has shown some single time step noise, so only considered when
|
||||
# switch is on for at least 2 consecutive CAN samples
|
||||
# panda safety only checks BRAKE_PRESSED signal
|
||||
ret.brakePressed = bool(cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or
|
||||
(self.brake_switch and self.brake_switch_prev and cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_prev_ts))
|
||||
ret.brakePressed = bool(cp.vl["POWERTRAIN_DATA"]["BRAKE_PRESSED"] or
|
||||
(self.brake_switch and self.brake_switch_prev and cp.ts["POWERTRAIN_DATA"]["BRAKE_SWITCH"] != self.brake_switch_prev_ts))
|
||||
|
||||
self.brake_switch_prev = self.brake_switch
|
||||
self.brake_switch_prev_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH']
|
||||
self.brake_switch_prev_ts = cp.ts["POWERTRAIN_DATA"]["BRAKE_SWITCH"]
|
||||
|
||||
ret.brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
|
||||
ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS'] != 0
|
||||
ret.brake = cp.vl["VSA_STATUS"]["USER_BRAKE"]
|
||||
ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]["ACC_STATUS"] != 0
|
||||
ret.cruiseState.available = bool(main_on)
|
||||
ret.cruiseState.nonAdaptive = self.cruise_mode != 0
|
||||
|
||||
@@ -337,7 +337,7 @@ class CarState(CarStateBase):
|
||||
ret.brakePressed = True
|
||||
|
||||
# TODO: discover the CAN msg that has the imperial unit bit for all other cars
|
||||
self.is_metric = not cp.vl["HUD_SETTING"]['IMPERIAL_UNIT'] if self.CP.carFingerprint in (CAR.CIVIC) else False
|
||||
self.is_metric = not cp.vl["HUD_SETTING"]["IMPERIAL_UNIT"] if self.CP.carFingerprint in (CAR.CIVIC) else False
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
ret.stockAeb = bool(cp.vl["ACC_CONTROL"]["AEB_STATUS"] and cp.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
|
||||
@@ -355,8 +355,8 @@ class CarState(CarStateBase):
|
||||
if self.CP.enableBsm and self.CP.carFingerprint in (CAR.CRV_5G, ):
|
||||
# BSM messages are on B-CAN, requires a panda forwarding B-CAN messages to CAN 0
|
||||
# more info here: https://github.com/commaai/openpilot/pull/1867
|
||||
ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]['BSM_ALERT'] == 1
|
||||
ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]['BSM_ALERT'] == 1
|
||||
ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1
|
||||
ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1
|
||||
|
||||
return ret
|
||||
|
||||
@@ -364,7 +364,7 @@ class CarState(CarStateBase):
|
||||
def get_can_parser(CP):
|
||||
signals, checks = get_can_signals(CP)
|
||||
bus_pt = 1 if CP.carFingerprint in HONDA_BOSCH else 0
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_pt)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, bus_pt)
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser(CP):
|
||||
@@ -389,7 +389,7 @@ class CarState(CarStateBase):
|
||||
("BRAKE_COMMAND", 50),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
|
||||
|
||||
@staticmethod
|
||||
def get_body_can_parser(CP):
|
||||
@@ -402,5 +402,5 @@ class CarState(CarStateBase):
|
||||
("BSM_STATUS_RIGHT", 3),
|
||||
]
|
||||
bus_body = 0 # B-CAN is forwarded to ACC-CAN radar side (CAN 0 on fake ethernet port)
|
||||
return CANParser(DBC[CP.carFingerprint]['body'], signals, checks, bus_body)
|
||||
return CANParser(DBC[CP.carFingerprint]["body"], signals, checks, bus_body)
|
||||
return None
|
||||
|
||||
@@ -12,55 +12,55 @@ class CarState(CarStateBase):
|
||||
def update(self, cp, cp_cam):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
ret.doorOpen = any([cp.vl["CGW1"]['CF_Gway_DrvDrSw'], cp.vl["CGW1"]['CF_Gway_AstDrSw'],
|
||||
cp.vl["CGW2"]['CF_Gway_RLDrSw'], cp.vl["CGW2"]['CF_Gway_RRDrSw']])
|
||||
ret.doorOpen = any([cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"],
|
||||
cp.vl["CGW2"]["CF_Gway_RLDrSw"], cp.vl["CGW2"]["CF_Gway_RRDrSw"]])
|
||||
|
||||
ret.seatbeltUnlatched = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] == 0
|
||||
ret.seatbeltUnlatched = cp.vl["CGW1"]["CF_Gway_DrvSeatBeltSw"] == 0
|
||||
|
||||
ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]["WHL_SPD_FL"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]["WHL_SPD_FR"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]["WHL_SPD_RL"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]["WHL_SPD_RR"] * CV.KPH_TO_MS
|
||||
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
|
||||
ret.standstill = ret.vEgoRaw < 0.1
|
||||
|
||||
ret.steeringAngleDeg = cp.vl["SAS11"]['SAS_Angle']
|
||||
ret.steeringRateDeg = cp.vl["SAS11"]['SAS_Speed']
|
||||
ret.yawRate = cp.vl["ESP12"]['YAW_RATE']
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["CGW1"]['CF_Gway_TurnSigLh'],
|
||||
cp.vl["CGW1"]['CF_Gway_TurnSigRh'])
|
||||
ret.steeringTorque = cp.vl["MDPS12"]['CR_Mdps_StrColTq']
|
||||
ret.steeringTorqueEps = cp.vl["MDPS12"]['CR_Mdps_OutTq']
|
||||
ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"]
|
||||
ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"]
|
||||
ret.yawRate = cp.vl["ESP12"]["YAW_RATE"]
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"],
|
||||
cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
|
||||
ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"]
|
||||
ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"]
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
|
||||
ret.steerWarning = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail'] != 0 or cp.vl["MDPS12"]['CF_Mdps_ToiFlt'] != 0
|
||||
ret.steerWarning = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
|
||||
|
||||
# cruise state
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
ret.cruiseState.available = cp.vl["TCS13"]['ACCEnable'] == 0
|
||||
ret.cruiseState.enabled = cp.vl["TCS13"]['ACC_REQ'] == 1
|
||||
ret.cruiseState.standstill = cp.vl["TCS13"]['StandStill'] == 1
|
||||
ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0
|
||||
ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
|
||||
ret.cruiseState.standstill = cp.vl["TCS13"]["StandStill"] == 1
|
||||
else:
|
||||
ret.cruiseState.available = cp.vl["SCC11"]['MainMode_ACC'] == 1
|
||||
ret.cruiseState.enabled = cp.vl["SCC12"]['ACCMode'] != 0
|
||||
ret.cruiseState.standstill = cp.vl["SCC11"]['SCCInfoDisplay'] == 4.
|
||||
ret.cruiseState.available = cp.vl["SCC11"]["MainMode_ACC"] == 1
|
||||
ret.cruiseState.enabled = cp.vl["SCC12"]["ACCMode"] != 0
|
||||
ret.cruiseState.standstill = cp.vl["SCC11"]["SCCInfoDisplay"] == 4.
|
||||
|
||||
if ret.cruiseState.enabled:
|
||||
speed_conv = CV.MPH_TO_MS if cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] else CV.KPH_TO_MS
|
||||
ret.cruiseState.speed = cp.vl["SCC11"]['VSetDis'] * speed_conv
|
||||
ret.cruiseState.speed = cp.vl["SCC11"]["VSetDis"] * speed_conv
|
||||
else:
|
||||
ret.cruiseState.speed = 0
|
||||
|
||||
# TODO: Find brake pressure
|
||||
ret.brake = 0
|
||||
ret.brakePressed = cp.vl["TCS13"]['DriverBraking'] != 0
|
||||
ret.brakePressed = cp.vl["TCS13"]["DriverBraking"] != 0
|
||||
|
||||
if self.CP.carFingerprint in EV_HYBRID:
|
||||
ret.gas = cp.vl["E_EMS11"]['Accel_Pedal_Pos'] / 256.
|
||||
ret.gas = cp.vl["E_EMS11"]["Accel_Pedal_Pos"] / 256.
|
||||
ret.gasPressed = ret.gas > 0
|
||||
else:
|
||||
ret.gas = cp.vl["EMS12"]['PV_AV_CAN'] / 100
|
||||
ret.gas = cp.vl["EMS12"]["PV_AV_CAN"] / 100
|
||||
ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"])
|
||||
|
||||
# TODO: refactor gear parsing in function
|
||||
@@ -116,11 +116,11 @@ class CarState(CarStateBase):
|
||||
ret.gearShifter = GearShifter.unknown
|
||||
|
||||
if self.CP.carFingerprint in FEATURES["use_fca"]:
|
||||
ret.stockAeb = cp.vl["FCA11"]['FCA_CmdAct'] != 0
|
||||
ret.stockFcw = cp.vl["FCA11"]['CF_VSM_Warn'] == 2
|
||||
ret.stockAeb = cp.vl["FCA11"]["FCA_CmdAct"] != 0
|
||||
ret.stockFcw = cp.vl["FCA11"]["CF_VSM_Warn"] == 2
|
||||
else:
|
||||
ret.stockAeb = cp.vl["SCC12"]['AEB_CmdAct'] != 0
|
||||
ret.stockFcw = cp.vl["SCC12"]['CF_VSM_Warn'] == 2
|
||||
ret.stockAeb = cp.vl["SCC12"]["AEB_CmdAct"] != 0
|
||||
ret.stockFcw = cp.vl["SCC12"]["CF_VSM_Warn"] == 2
|
||||
|
||||
if self.CP.enableBsm:
|
||||
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
|
||||
@@ -129,11 +129,11 @@ class CarState(CarStateBase):
|
||||
# save the entire LKAS11 and CLU11
|
||||
self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
|
||||
self.clu11 = copy.copy(cp.vl["CLU11"])
|
||||
self.park_brake = cp.vl["TCS13"]['PBRAKE_ACT'] == 1
|
||||
self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] # 0 NOT ACTIVE, 1 ACTIVE
|
||||
self.lead_distance = cp.vl["SCC11"]['ACC_ObjDist']
|
||||
self.brake_hold = cp.vl["TCS15"]['AVH_LAMP'] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
|
||||
self.brake_error = cp.vl["TCS13"]['ACCEnable'] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
|
||||
self.park_brake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
|
||||
self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE
|
||||
self.lead_distance = cp.vl["SCC11"]["ACC_ObjDist"]
|
||||
self.brake_hold = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
|
||||
self.brake_error = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
|
||||
self.prev_cruise_buttons = self.cruise_buttons
|
||||
self.cruise_buttons = cp.vl["CLU11"]["CF_Clu_CruiseSwState"]
|
||||
|
||||
@@ -286,7 +286,7 @@ class CarState(CarStateBase):
|
||||
("CF_VSM_Warn", "SCC12", 0),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser(CP):
|
||||
@@ -314,4 +314,4 @@ class CarState(CarStateBase):
|
||||
("LKAS11", 100)
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
|
||||
|
||||
@@ -9,8 +9,8 @@ class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
self.shifter_values = can_define.dv["GEAR"]['GEAR']
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
self.shifter_values = can_define.dv["GEAR"]["GEAR"]
|
||||
|
||||
self.cruise_speed = 0
|
||||
self.acc_active_last = False
|
||||
@@ -21,42 +21,42 @@ class CarState(CarStateBase):
|
||||
def update(self, cp, cp_cam):
|
||||
|
||||
ret = car.CarState.new_message()
|
||||
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['FL'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['FR'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]['RL'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]['RR'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["FL"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["FR"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["RL"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["RR"] * CV.KPH_TO_MS
|
||||
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
|
||||
# Match panda speed reading
|
||||
speed_kph = cp.vl["ENGINE_DATA"]['SPEED']
|
||||
speed_kph = cp.vl["ENGINE_DATA"]["SPEED"]
|
||||
ret.standstill = speed_kph < .1
|
||||
|
||||
can_gear = int(cp.vl["GEAR"]['GEAR'])
|
||||
can_gear = int(cp.vl["GEAR"]["GEAR"])
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
|
||||
|
||||
ret.leftBlinker = cp.vl["BLINK_INFO"]['LEFT_BLINK'] == 1
|
||||
ret.rightBlinker = cp.vl["BLINK_INFO"]['RIGHT_BLINK'] == 1
|
||||
ret.leftBlinker = cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1
|
||||
ret.rightBlinker = cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1
|
||||
|
||||
ret.steeringAngleDeg = cp.vl["STEER"]['STEER_ANGLE']
|
||||
ret.steeringTorque = cp.vl["STEER_TORQUE"]['STEER_TORQUE_SENSOR']
|
||||
ret.steeringAngleDeg = cp.vl["STEER"]["STEER_ANGLE"]
|
||||
ret.steeringTorque = cp.vl["STEER_TORQUE"]["STEER_TORQUE_SENSOR"]
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD
|
||||
|
||||
ret.steeringTorqueEps = cp.vl["STEER_TORQUE"]['STEER_TORQUE_MOTOR']
|
||||
ret.steeringRateDeg = cp.vl["STEER_RATE"]['STEER_ANGLE_RATE']
|
||||
ret.steeringTorqueEps = cp.vl["STEER_TORQUE"]["STEER_TORQUE_MOTOR"]
|
||||
ret.steeringRateDeg = cp.vl["STEER_RATE"]["STEER_ANGLE_RATE"]
|
||||
|
||||
ret.brakePressed = cp.vl["PEDALS"]['BRAKE_ON'] == 1
|
||||
ret.brake = cp.vl["BRAKE"]['BRAKE_PRESSURE']
|
||||
ret.brakePressed = cp.vl["PEDALS"]["BRAKE_ON"] == 1
|
||||
ret.brake = cp.vl["BRAKE"]["BRAKE_PRESSURE"]
|
||||
|
||||
ret.seatbeltUnlatched = cp.vl["SEATBELT"]['DRIVER_SEATBELT'] == 0
|
||||
ret.doorOpen = any([cp.vl["DOORS"]['FL'], cp.vl["DOORS"]['FR'],
|
||||
cp.vl["DOORS"]['BL'], cp.vl["DOORS"]['BR']])
|
||||
ret.seatbeltUnlatched = cp.vl["SEATBELT"]["DRIVER_SEATBELT"] == 0
|
||||
ret.doorOpen = any([cp.vl["DOORS"]["FL"], cp.vl["DOORS"]["FR"],
|
||||
cp.vl["DOORS"]["BL"], cp.vl["DOORS"]["BR"]])
|
||||
|
||||
ret.gas = cp.vl["ENGINE_DATA"]['PEDAL_GAS']
|
||||
ret.gas = cp.vl["ENGINE_DATA"]["PEDAL_GAS"]
|
||||
ret.gasPressed = ret.gas > 0
|
||||
|
||||
ret.leftBlindspot = cp.vl["BSM"]['LEFT_BS1'] == 1
|
||||
ret.rightBlindspot = cp.vl["BSM"]['RIGHT_BS1'] == 1
|
||||
ret.leftBlindspot = cp.vl["BSM"]["LEFT_BS1"] == 1
|
||||
ret.rightBlindspot = cp.vl["BSM"]["RIGHT_BS1"] == 1
|
||||
|
||||
# LKAS is enabled at 52kph going up and disabled at 45kph going down
|
||||
if speed_kph > LKAS_LIMITS.ENABLE_SPEED:
|
||||
@@ -65,13 +65,13 @@ class CarState(CarStateBase):
|
||||
self.lkas_allowed = False
|
||||
|
||||
# if any of the cruize buttons is pressed force state update
|
||||
if any([cp.vl["CRZ_BTNS"]['RES'],
|
||||
cp.vl["CRZ_BTNS"]['SET_P'],
|
||||
cp.vl["CRZ_BTNS"]['SET_M']]):
|
||||
if any([cp.vl["CRZ_BTNS"]["RES"],
|
||||
cp.vl["CRZ_BTNS"]["SET_P"],
|
||||
cp.vl["CRZ_BTNS"]["SET_M"]]):
|
||||
self.cruise_speed = ret.vEgoRaw
|
||||
|
||||
ret.cruiseState.available = True
|
||||
ret.cruiseState.enabled = cp.vl["CRZ_CTRL"]['CRZ_ACTIVE'] == 1
|
||||
ret.cruiseState.enabled = cp.vl["CRZ_CTRL"]["CRZ_ACTIVE"] == 1
|
||||
ret.cruiseState.speed = self.cruise_speed
|
||||
|
||||
if ret.cruiseState.enabled:
|
||||
@@ -85,12 +85,12 @@ class CarState(CarStateBase):
|
||||
self.low_speed_alert = False
|
||||
|
||||
# On if no driver torque the last 5 seconds
|
||||
ret.steerWarning = cp.vl["STEER_RATE"]['HANDS_OFF_5_SECONDS'] == 1
|
||||
ret.steerWarning = cp.vl["STEER_RATE"]["HANDS_OFF_5_SECONDS"] == 1
|
||||
|
||||
self.acc_active_last = ret.cruiseState.enabled
|
||||
|
||||
self.cam_lkas = cp_cam.vl["CAM_LKAS"]
|
||||
ret.steerError = cp_cam.vl["CAM_LKAS"]['ERR_BIT_1'] == 1
|
||||
ret.steerError = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
|
||||
|
||||
return ret
|
||||
|
||||
@@ -157,7 +157,7 @@ class CarState(CarStateBase):
|
||||
("BSM", 10),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser(CP):
|
||||
@@ -184,4 +184,4 @@ class CarState(CarStateBase):
|
||||
("CAM_LKAS", 16),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
|
||||
|
||||
@@ -12,7 +12,7 @@ TORQUE_SAMPLES = 12
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
|
||||
self.lkas_hud_msg = None
|
||||
self.lkas_hud_info_msg = None
|
||||
@@ -216,7 +216,7 @@ class CarState(CarStateBase):
|
||||
("LKAS_SETTINGS", 10),
|
||||
("PROPILOT_HUD", 50),
|
||||
]
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 1)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1)
|
||||
|
||||
signals += [
|
||||
("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
|
||||
@@ -225,7 +225,7 @@ class CarState(CarStateBase):
|
||||
("STEER_TORQUE_SENSOR", 100),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
|
||||
|
||||
@staticmethod
|
||||
def get_adas_can_parser(CP):
|
||||
@@ -337,7 +337,7 @@ class CarState(CarStateBase):
|
||||
("LKAS", 100),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser(CP):
|
||||
@@ -358,7 +358,7 @@ class CarState(CarStateBase):
|
||||
checks += [
|
||||
("STEER_TORQUE_SENSOR", 100),
|
||||
]
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
|
||||
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 1)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1)
|
||||
|
||||
@@ -10,68 +10,68 @@ from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD, CAR, PREGLOBAL_CAR
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
self.shifter_values = can_define.dv["Transmission"]['Gear']
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
self.shifter_values = can_define.dv["Transmission"]["Gear"]
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
ret.gas = cp.vl["Throttle"]['Throttle_Pedal'] / 255.
|
||||
ret.gas = cp.vl["Throttle"]["Throttle_Pedal"] / 255.
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
if self.car_fingerprint in PREGLOBAL_CARS:
|
||||
ret.brakePressed = cp.vl["Brake_Pedal"]['Brake_Pedal'] > 2
|
||||
ret.brakePressed = cp.vl["Brake_Pedal"]["Brake_Pedal"] > 2
|
||||
else:
|
||||
ret.brakePressed = cp.vl["Brake_Pedal"]['Brake_Pedal'] > 1e-5
|
||||
ret.brakePressed = cp.vl["Brake_Pedal"]["Brake_Pedal"] > 1e-5
|
||||
|
||||
ret.wheelSpeeds.fl = cp.vl["Wheel_Speeds"]['FL'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = cp.vl["Wheel_Speeds"]['FR'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = cp.vl["Wheel_Speeds"]['RL'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fl = cp.vl["Wheel_Speeds"]["FL"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = cp.vl["Wheel_Speeds"]["FR"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = cp.vl["Wheel_Speeds"]["RL"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = cp.vl["Wheel_Speeds"]["RR"] * CV.KPH_TO_MS
|
||||
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
|
||||
# Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = ret.vEgoRaw < 0.01
|
||||
|
||||
# continuous blinker signals for assisted lane change
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["Dashlights"]['LEFT_BLINKER'],
|
||||
cp.vl["Dashlights"]['RIGHT_BLINKER'])
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["Dashlights"]["LEFT_BLINKER"],
|
||||
cp.vl["Dashlights"]["RIGHT_BLINKER"])
|
||||
|
||||
if self.CP.enableBsm:
|
||||
ret.leftBlindspot = (cp.vl["BSD_RCTA"]['L_ADJACENT'] == 1) or (cp.vl["BSD_RCTA"]['L_APPROACHING'] == 1)
|
||||
ret.rightBlindspot = (cp.vl["BSD_RCTA"]['R_ADJACENT'] == 1) or (cp.vl["BSD_RCTA"]['R_APPROACHING'] == 1)
|
||||
ret.leftBlindspot = (cp.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["L_APPROACHING"] == 1)
|
||||
ret.rightBlindspot = (cp.vl["BSD_RCTA"]["R_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["R_APPROACHING"] == 1)
|
||||
|
||||
can_gear = int(cp.vl["Transmission"]['Gear'])
|
||||
can_gear = int(cp.vl["Transmission"]["Gear"])
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
|
||||
|
||||
ret.steeringAngleDeg = cp.vl["Steering_Torque"]['Steering_Angle']
|
||||
ret.steeringTorque = cp.vl["Steering_Torque"]['Steer_Torque_Sensor']
|
||||
ret.steeringAngleDeg = cp.vl["Steering_Torque"]["Steering_Angle"]
|
||||
ret.steeringTorque = cp.vl["Steering_Torque"]["Steer_Torque_Sensor"]
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.car_fingerprint]
|
||||
|
||||
ret.cruiseState.enabled = cp.vl["CruiseControl"]['Cruise_Activated'] != 0
|
||||
ret.cruiseState.available = cp.vl["CruiseControl"]['Cruise_On'] != 0
|
||||
ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]['Cruise_Set_Speed'] * CV.KPH_TO_MS
|
||||
ret.cruiseState.enabled = cp.vl["CruiseControl"]["Cruise_Activated"] != 0
|
||||
ret.cruiseState.available = cp.vl["CruiseControl"]["Cruise_On"] != 0
|
||||
ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS
|
||||
|
||||
# UDM Forester, Legacy: mph = 0
|
||||
if self.car_fingerprint in [CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL] and cp.vl["Dash_State"]['Units'] == 0:
|
||||
if self.car_fingerprint in [CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL] and cp.vl["Dash_State"]["Units"] == 0:
|
||||
ret.cruiseState.speed *= CV.MPH_TO_KPH
|
||||
# EDM Global: mph = 1, 2; All Outback: mph = 1, UDM Forester: mph = 7
|
||||
elif self.car_fingerprint not in [CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL] and cp.vl["Dash_State"]['Units'] in [1, 2, 7]:
|
||||
elif self.car_fingerprint not in [CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL] and cp.vl["Dash_State"]["Units"] in [1, 2, 7]:
|
||||
ret.cruiseState.speed *= CV.MPH_TO_KPH
|
||||
|
||||
ret.seatbeltUnlatched = cp.vl["Dashlights"]['SEATBELT_FL'] == 1
|
||||
ret.doorOpen = any([cp.vl["BodyInfo"]['DOOR_OPEN_RR'],
|
||||
cp.vl["BodyInfo"]['DOOR_OPEN_RL'],
|
||||
cp.vl["BodyInfo"]['DOOR_OPEN_FR'],
|
||||
cp.vl["BodyInfo"]['DOOR_OPEN_FL']])
|
||||
ret.steerError = cp.vl["Steering_Torque"]['Steer_Error_1'] == 1
|
||||
ret.seatbeltUnlatched = cp.vl["Dashlights"]["SEATBELT_FL"] == 1
|
||||
ret.doorOpen = any([cp.vl["BodyInfo"]["DOOR_OPEN_RR"],
|
||||
cp.vl["BodyInfo"]["DOOR_OPEN_RL"],
|
||||
cp.vl["BodyInfo"]["DOOR_OPEN_FR"],
|
||||
cp.vl["BodyInfo"]["DOOR_OPEN_FL"]])
|
||||
ret.steerError = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1
|
||||
|
||||
if self.car_fingerprint in PREGLOBAL_CARS:
|
||||
self.cruise_button = cp_cam.vl["ES_CruiseThrottle"]["Cruise_Button"]
|
||||
self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"]
|
||||
self.es_accel_msg = copy.copy(cp_cam.vl["ES_CruiseThrottle"])
|
||||
else:
|
||||
ret.steerWarning = cp.vl["Steering_Torque"]['Steer_Warning'] == 1
|
||||
ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]['Conventional_Cruise'] == 1
|
||||
ret.steerWarning = cp.vl["Steering_Torque"]["Steer_Warning"] == 1
|
||||
ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]["Conventional_Cruise"] == 1
|
||||
self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
|
||||
self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
|
||||
|
||||
@@ -151,7 +151,7 @@ class CarState(CarStateBase):
|
||||
("CruiseControl", 50),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser(CP):
|
||||
@@ -231,4 +231,4 @@ class CarState(CarStateBase):
|
||||
("ES_LKAS_State", 10),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
|
||||
|
||||
@@ -10,10 +10,10 @@ from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
self.shifter_values = can_define.dv["GEAR_PACKET"]['GEAR']
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"]
|
||||
|
||||
# On cars with cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
|
||||
# On cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
|
||||
# the signal is zeroed to where the steering angle is at start.
|
||||
# Need to apply an offset as soon as the steering angle measurements are both received
|
||||
self.needs_angle_offset = True
|
||||
@@ -23,82 +23,82 @@ class CarState(CarStateBase):
|
||||
def update(self, cp, cp_cam):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
ret.doorOpen = any([cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'],
|
||||
cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']])
|
||||
ret.seatbeltUnlatched = cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED'] != 0
|
||||
ret.doorOpen = any([cp.vl["SEATS_DOORS"]["DOOR_OPEN_FL"], cp.vl["SEATS_DOORS"]["DOOR_OPEN_FR"],
|
||||
cp.vl["SEATS_DOORS"]["DOOR_OPEN_RL"], cp.vl["SEATS_DOORS"]["DOOR_OPEN_RR"]])
|
||||
ret.seatbeltUnlatched = cp.vl["SEATS_DOORS"]["SEATBELT_DRIVER_UNLATCHED"] != 0
|
||||
|
||||
ret.brakePressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] != 0
|
||||
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
|
||||
if self.CP.enableGasInterceptor:
|
||||
ret.gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
|
||||
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
|
||||
ret.gasPressed = ret.gas > 15
|
||||
else:
|
||||
ret.gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
|
||||
ret.gasPressed = cp.vl["PCM_CRUISE"]['GAS_RELEASED'] == 0
|
||||
ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"]
|
||||
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
|
||||
|
||||
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"] * CV.KPH_TO_MS
|
||||
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
|
||||
ret.standstill = ret.vEgoRaw < 0.001
|
||||
|
||||
# Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
|
||||
if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3:
|
||||
if abs(cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]) > 1e-3:
|
||||
self.accurate_steer_angle_seen = True
|
||||
|
||||
if self.accurate_steer_angle_seen:
|
||||
ret.steeringAngleDeg = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
|
||||
ret.steeringAngleDeg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] - self.angle_offset
|
||||
if self.needs_angle_offset:
|
||||
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
|
||||
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
|
||||
if abs(angle_wheel) > 1e-3:
|
||||
self.needs_angle_offset = False
|
||||
self.angle_offset = ret.steeringAngleDeg - angle_wheel
|
||||
else:
|
||||
ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
|
||||
ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
|
||||
|
||||
ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
|
||||
ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
|
||||
|
||||
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
|
||||
can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
|
||||
ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
|
||||
ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
|
||||
ret.leftBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1
|
||||
ret.rightBlinker = cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2
|
||||
|
||||
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
|
||||
ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS']
|
||||
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"]
|
||||
ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"]
|
||||
# we could use the override bit from dbc, but it's triggered at too high torque values
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
|
||||
ret.steerWarning = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
|
||||
ret.steerWarning = cp.vl["EPS_STATUS"]["LKA_STATE"] not in [1, 5]
|
||||
|
||||
if self.CP.carFingerprint == CAR.LEXUS_IS:
|
||||
ret.cruiseState.available = cp.vl["DSU_CRUISE"]['MAIN_ON'] != 0
|
||||
ret.cruiseState.speed = cp.vl["DSU_CRUISE"]['SET_SPEED'] * CV.KPH_TO_MS
|
||||
ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
|
||||
ret.cruiseState.speed = cp.vl["DSU_CRUISE"]["SET_SPEED"] * CV.KPH_TO_MS
|
||||
self.low_speed_lockout = False
|
||||
else:
|
||||
ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]['MAIN_ON'] != 0
|
||||
ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]['SET_SPEED'] * CV.KPH_TO_MS
|
||||
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
|
||||
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
|
||||
ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0
|
||||
ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]["SET_SPEED"] * CV.KPH_TO_MS
|
||||
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2
|
||||
self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"]
|
||||
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
|
||||
# ignore standstill in hybrid vehicles, since pcm allows to restart without
|
||||
# receiving any special command. Also if interceptor is detected
|
||||
ret.cruiseState.standstill = False
|
||||
else:
|
||||
ret.cruiseState.standstill = self.pcm_acc_status == 7
|
||||
ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
|
||||
ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]['CRUISE_STATE'] in [1, 2, 3, 4, 5, 6]
|
||||
ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"])
|
||||
ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] in [1, 2, 3, 4, 5, 6]
|
||||
|
||||
ret.genericToggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
|
||||
ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"])
|
||||
ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
|
||||
|
||||
ret.espDisabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] != 0
|
||||
ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0
|
||||
# 2 is standby, 10 is active. TODO: check that everything else is really a faulty state
|
||||
self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE']
|
||||
self.steer_state = cp.vl["EPS_STATUS"]["LKA_STATE"]
|
||||
|
||||
if self.CP.enableBsm:
|
||||
ret.leftBlindspot = (cp.vl["BSM"]['L_ADJACENT'] == 1) or (cp.vl["BSM"]['L_APPROACHING'] == 1)
|
||||
ret.rightBlindspot = (cp.vl["BSM"]['R_ADJACENT'] == 1) or (cp.vl["BSM"]['R_APPROACHING'] == 1)
|
||||
ret.leftBlindspot = (cp.vl["BSM"]["L_ADJACENT"] == 1) or (cp.vl["BSM"]["L_APPROACHING"] == 1)
|
||||
ret.rightBlindspot = (cp.vl["BSM"]["R_ADJACENT"] == 1) or (cp.vl["BSM"]["R_APPROACHING"] == 1)
|
||||
|
||||
return ret
|
||||
|
||||
@@ -176,7 +176,7 @@ class CarState(CarStateBase):
|
||||
("BSM", 1)
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser(CP):
|
||||
@@ -192,4 +192,4 @@ class CarState(CarStateBase):
|
||||
("PRE_COLLISION", 0), # TODO: figure out why freq is inconsistent
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2)
|
||||
|
||||
@@ -9,20 +9,20 @@ from selfdrive.car.volkswagen.values import DBC, CANBUS, TransmissionType, GearS
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
if CP.transmissionType == TransmissionType.automatic:
|
||||
self.shifter_values = can_define.dv["Getriebe_11"]['GE_Fahrstufe']
|
||||
self.shifter_values = can_define.dv["Getriebe_11"]["GE_Fahrstufe"]
|
||||
elif CP.transmissionType == TransmissionType.direct:
|
||||
self.shifter_values = can_define.dv["EV_Gearshift"]['GearPosition']
|
||||
self.shifter_values = can_define.dv["EV_Gearshift"]["GearPosition"]
|
||||
self.buttonStates = BUTTON_STATES.copy()
|
||||
|
||||
def update(self, pt_cp, cam_cp, trans_type):
|
||||
ret = car.CarState.new_message()
|
||||
# Update vehicle speed and acceleration from ABS wheel speeds.
|
||||
ret.wheelSpeeds.fl = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = pt_cp.vl["ESP_19"]['ESP_VR_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = pt_cp.vl["ESP_19"]['ESP_HL_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = pt_cp.vl["ESP_19"]['ESP_HR_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fl = pt_cp.vl["ESP_19"]["ESP_VL_Radgeschw_02"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = pt_cp.vl["ESP_19"]["ESP_VR_Radgeschw_02"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = pt_cp.vl["ESP_19"]["ESP_HL_Radgeschw_02"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = pt_cp.vl["ESP_19"]["ESP_HR_Radgeschw_02"] * CV.KPH_TO_MS
|
||||
|
||||
ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]))
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
@@ -31,36 +31,36 @@ class CarState(CarStateBase):
|
||||
|
||||
# Update steering angle, rate, yaw rate, and driver input torque. VW send
|
||||
# the sign/direction in a separate signal so they must be recombined.
|
||||
ret.steeringAngleDeg = pt_cp.vl["LH_EPS_03"]['EPS_Berechneter_LW'] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]['EPS_VZ_BLW'])]
|
||||
ret.steeringRateDeg = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1, -1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradw_Geschw'])]
|
||||
ret.steeringTorque = pt_cp.vl["LH_EPS_03"]['EPS_Lenkmoment'] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]['EPS_VZ_Lenkmoment'])]
|
||||
ret.steeringAngleDeg = pt_cp.vl["LH_EPS_03"]["EPS_Berechneter_LW"] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]["EPS_VZ_BLW"])]
|
||||
ret.steeringRateDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradw_Geschw"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradw_Geschw"])]
|
||||
ret.steeringTorque = pt_cp.vl["LH_EPS_03"]["EPS_Lenkmoment"] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]["EPS_VZ_Lenkmoment"])]
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
|
||||
ret.yawRate = pt_cp.vl["ESP_02"]['ESP_Gierrate'] * (1, -1)[int(pt_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD
|
||||
ret.yawRate = pt_cp.vl["ESP_02"]["ESP_Gierrate"] * (1, -1)[int(pt_cp.vl["ESP_02"]["ESP_VZ_Gierrate"])] * CV.DEG_TO_RAD
|
||||
|
||||
# Update gas, brakes, and gearshift.
|
||||
ret.gas = pt_cp.vl["Motor_20"]['MO_Fahrpedalrohwert_01'] / 100.0
|
||||
ret.gas = pt_cp.vl["Motor_20"]["MO_Fahrpedalrohwert_01"] / 100.0
|
||||
ret.gasPressed = ret.gas > 0
|
||||
ret.brake = pt_cp.vl["ESP_05"]['ESP_Bremsdruck'] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects
|
||||
ret.brakePressed = bool(pt_cp.vl["ESP_05"]['ESP_Fahrer_bremst'])
|
||||
ret.brake = pt_cp.vl["ESP_05"]["ESP_Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects
|
||||
ret.brakePressed = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"])
|
||||
|
||||
# Update gear and/or clutch position data.
|
||||
if trans_type == TransmissionType.automatic:
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["Getriebe_11"]['GE_Fahrstufe'], None))
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["Getriebe_11"]["GE_Fahrstufe"], None))
|
||||
elif trans_type == TransmissionType.direct:
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["EV_Gearshift"]['GearPosition'], None))
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["EV_Gearshift"]["GearPosition"], None))
|
||||
elif trans_type == TransmissionType.manual:
|
||||
ret.clutchPressed = not pt_cp.vl["Motor_14"]['MO_Kuppl_schalter']
|
||||
if bool(pt_cp.vl["Gateway_72"]['BCM1_Rueckfahrlicht_Schalter']):
|
||||
ret.clutchPressed = not pt_cp.vl["Motor_14"]["MO_Kuppl_schalter"]
|
||||
if bool(pt_cp.vl["Gateway_72"]["BCM1_Rueckfahrlicht_Schalter"]):
|
||||
ret.gearShifter = GearShifter.reverse
|
||||
else:
|
||||
ret.gearShifter = GearShifter.drive
|
||||
|
||||
# Update door and trunk/hatch lid open status.
|
||||
ret.doorOpen = any([pt_cp.vl["Gateway_72"]['ZV_FT_offen'],
|
||||
pt_cp.vl["Gateway_72"]['ZV_BT_offen'],
|
||||
pt_cp.vl["Gateway_72"]['ZV_HFS_offen'],
|
||||
pt_cp.vl["Gateway_72"]['ZV_HBFS_offen'],
|
||||
pt_cp.vl["Gateway_72"]['ZV_HD_offen']])
|
||||
ret.doorOpen = any([pt_cp.vl["Gateway_72"]["ZV_FT_offen"],
|
||||
pt_cp.vl["Gateway_72"]["ZV_BT_offen"],
|
||||
pt_cp.vl["Gateway_72"]["ZV_HFS_offen"],
|
||||
pt_cp.vl["Gateway_72"]["ZV_HBFS_offen"],
|
||||
pt_cp.vl["Gateway_72"]["ZV_HD_offen"]])
|
||||
|
||||
# Update seatbelt fastened status.
|
||||
ret.seatbeltUnlatched = pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] != 3
|
||||
@@ -100,7 +100,7 @@ class CarState(CarStateBase):
|
||||
ret.stockAeb = bool(pt_cp.vl["ACC_10"]["ANB_Teilbremsung_Freigabe"]) or bool(pt_cp.vl["ACC_10"]["ANB_Zielbremsung_Freigabe"])
|
||||
|
||||
# Update ACC radar status.
|
||||
accStatus = pt_cp.vl["TSK_06"]['TSK_Status']
|
||||
accStatus = pt_cp.vl["TSK_06"]["TSK_Status"]
|
||||
if accStatus == 2:
|
||||
# ACC okay and enabled, but not currently engaged
|
||||
ret.cruiseState.available = True
|
||||
@@ -116,38 +116,38 @@ class CarState(CarStateBase):
|
||||
|
||||
# Update ACC setpoint. When the setpoint is zero or there's an error, the
|
||||
# radar sends a set-speed of ~90.69 m/s / 203mph.
|
||||
ret.cruiseState.speed = pt_cp.vl["ACC_02"]['ACC_Wunschgeschw'] * CV.KPH_TO_MS
|
||||
ret.cruiseState.speed = pt_cp.vl["ACC_02"]["ACC_Wunschgeschw"] * CV.KPH_TO_MS
|
||||
if ret.cruiseState.speed > 90:
|
||||
ret.cruiseState.speed = 0
|
||||
|
||||
# Update control button states for turn signals and ACC controls.
|
||||
self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Hoch'])
|
||||
self.buttonStates["decelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Runter'])
|
||||
self.buttonStates["cancel"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Abbrechen'])
|
||||
self.buttonStates["setCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Setzen'])
|
||||
self.buttonStates["resumeCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Wiederaufnahme'])
|
||||
self.buttonStates["gapAdjustCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Verstellung_Zeitluecke'])
|
||||
ret.leftBlinker = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_li'])
|
||||
ret.rightBlinker = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_re'])
|
||||
self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Hoch"])
|
||||
self.buttonStates["decelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Runter"])
|
||||
self.buttonStates["cancel"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Abbrechen"])
|
||||
self.buttonStates["setCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Setzen"])
|
||||
self.buttonStates["resumeCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Wiederaufnahme"])
|
||||
self.buttonStates["gapAdjustCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Verstellung_Zeitluecke"])
|
||||
ret.leftBlinker = bool(pt_cp.vl["Gateway_72"]["BH_Blinker_li"])
|
||||
ret.rightBlinker = bool(pt_cp.vl["Gateway_72"]["BH_Blinker_re"])
|
||||
|
||||
# Read ACC hardware button type configuration info that has to pass thru
|
||||
# to the radar. Ends up being different for steering wheel buttons vs
|
||||
# third stalk type controls.
|
||||
self.graHauptschalter = pt_cp.vl["GRA_ACC_01"]['GRA_Hauptschalter']
|
||||
self.graTypHauptschalter = pt_cp.vl["GRA_ACC_01"]['GRA_Typ_Hauptschalter']
|
||||
self.graButtonTypeInfo = pt_cp.vl["GRA_ACC_01"]['GRA_ButtonTypeInfo']
|
||||
self.graTipStufe2 = pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Stufe_2']
|
||||
self.graHauptschalter = pt_cp.vl["GRA_ACC_01"]["GRA_Hauptschalter"]
|
||||
self.graTypHauptschalter = pt_cp.vl["GRA_ACC_01"]["GRA_Typ_Hauptschalter"]
|
||||
self.graButtonTypeInfo = pt_cp.vl["GRA_ACC_01"]["GRA_ButtonTypeInfo"]
|
||||
self.graTipStufe2 = pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Stufe_2"]
|
||||
# Pick up the GRA_ACC_01 CAN message counter so we can sync to it for
|
||||
# later cruise-control button spamming.
|
||||
self.graMsgBusCounter = pt_cp.vl["GRA_ACC_01"]['COUNTER']
|
||||
self.graMsgBusCounter = pt_cp.vl["GRA_ACC_01"]["COUNTER"]
|
||||
|
||||
# Check to make sure the electric power steering rack is configured to
|
||||
# accept and respond to HCA_01 messages and has not encountered a fault.
|
||||
self.steeringFault = not pt_cp.vl["LH_EPS_03"]["EPS_HCA_Status"]
|
||||
|
||||
# Additional safety checks performed in CarInterface.
|
||||
self.parkingBrakeSet = bool(pt_cp.vl["Kombi_01"]['KBI_Handbremse']) # FIXME: need to include an EPB check as well
|
||||
ret.espDisabled = pt_cp.vl["ESP_21"]['ESP_Tastung_passiv'] != 0
|
||||
self.parkingBrakeSet = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"]) # FIXME: need to include an EPB check as well
|
||||
ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0
|
||||
|
||||
return ret
|
||||
|
||||
@@ -236,7 +236,7 @@ class CarState(CarStateBase):
|
||||
signals += MqbExtraSignals.bsm_radar_signals
|
||||
checks += MqbExtraSignals.bsm_radar_checks
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, CANBUS.pt)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.pt)
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser(CP):
|
||||
@@ -255,7 +255,7 @@ class CarState(CarStateBase):
|
||||
("LDW_02", 10) # From R242 Driver assistance camera
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, CANBUS.cam)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.cam)
|
||||
|
||||
class MqbExtraSignals:
|
||||
# Additional signal and message lists for optional or bus-portable controllers
|
||||
|
||||
Reference in New Issue
Block a user