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