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']
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.steeringAngle = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
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.gasPressed = ret.gas > 1e-5
ret.steeringTorque = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
# 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)
# 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
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']
regen_pressed = False
if self.car_fingerprint == CAR.VOLT:
regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
# Regen braking is braking
ret.brakePressed = ret.brake > 1e-5 or regen_pressed
ret.cruiseState.enabled = self.pcm_acc_status != AccState.OFF
ret.cruiseState.standstill = self.pcm_acc_status == AccState.STANDSTILL
# 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed
self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus']
ret.steerWarning = not is_eps_status_ok(self.lkas_status, self.car_fingerprint)
return ret
def update(self, pt_cp):
self.prev_cruise_buttons = self.cruise_buttons
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons']
self.prev_lka_button = self.lka_button
self.lka_button = pt_cp.vl["ASCMSteeringButton"]["LKAButton"]
self.prev_distance_button = self.distance_button
self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"]
self.v_wheel_fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_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.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.user_brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition']
self.pedal_gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal']
self.user_gas_pressed = self.pedal_gas > 0
self.steer_torque_driver = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
# 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed
self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus']
self.steer_not_allowed = not is_eps_status_ok(self.lkas_status, self.car_fingerprint)
# 1 - open, 0 - closed
self.door_all_closed = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 0 and
pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 0 and
pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 0 and
pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 0)
# 1 - latched
self.seatbelt = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 1
self.steer_error = False
self.brake_error = False
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
self.left_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1
self.right_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2
if self.car_fingerprint in SUPERCRUISE_CARS:
self.park_brake = False
self.main_on = False
self.acc_active = pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive']
self.esp_disabled = False
self.regen_pressed = False
self.pcm_acc_status = int(self.acc_active)
else:
self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed']
self.main_on = pt_cp.vl["ECMEngineStatus"]['CruiseMainOn']
self.acc_active = False
self.esp_disabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1
self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState']
if self.car_fingerprint == CAR.VOLT:
self.regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
else:
self.regen_pressed = False
# Brake pedal's potentiometer returns near-zero reading
# even when pedal is not pressed.
if self.user_brake < 10:
self.user_brake = 0
# Regen braking is braking
self.brake_pressed = self.user_brake > 10 or self.regen_pressed
self.gear_shifter_valid = self.gear_shifter == car.CarState.GearShifter.drive
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']
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.steeringAngle = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
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.
# # TODO: need a better way to identify cars without ACC
# # TODO: this assumes the Pedal is present. If it isn't, this won't work...
# if self.CP.carFingerprint in (CAR.BOLT):
# ret.gas = self.pedal_gas / 256.
# else:
ret.gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal'] / 254.
# 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 = (pt_cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] +
pt_cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
self.user_gas_pressed = self.user_gas > 20 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
ret.gasPressed = self.user_gas_pressed
#workaround for insta-disengage
#ret.gasPressed = False
else:
ret.gasPressed = ret.gas > 1e-5
#ret.gasPressed = ret.gas > 1e-5
ret.steeringTorque = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
# 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)
# 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
if self.car_fingerprint in SUPERCRUISE_CARS:
self.park_brake = False
ret.cruiseState.available = False
ret.espDisabled = False
regen_pressed = False
self.pcm_acc_status = int(
pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive'])
else:
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']
if self.car_fingerprint == CAR.VOLT or self.car_fingerprint == CAR.BOLT:
regen_pressed = bool(
pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle'])
else:
regen_pressed = False
# Regen braking is braking
ret.brakePressed = ret.brake > 1e-5 or regen_pressed
ret.cruiseState.enabled = self.pcm_acc_status != AccState.OFF
ret.cruiseState.standstill = self.pcm_acc_status == AccState.STANDSTILL
# 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed
self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus']
self.steer_warning = not is_eps_status_ok(self.lkas_status,
self.car_fingerprint)
self.steer_not_allowed = not is_eps_status_ok(self.lkas_status,
self.car_fingerprint)
return ret
