mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-06-12 05:44:14 +08:00
fix broken toyota
This commit is contained in:
dragonpilot
@@ -1,219 +1,219 @@
|
||||
from common.numpy_fast import mean
|
||||
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
|
||||
from common.realtime import sec_since_boot
|
||||
from common.params import Params
|
||||
params = Params()
|
||||
|
||||
|
||||
def get_can_parser(CP):
|
||||
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0),
|
||||
("GEAR", "GEAR_PACKET", 0),
|
||||
("BRAKE_PRESSED", "BRAKE_MODULE", 0),
|
||||
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
|
||||
("DOOR_OPEN_FL", "SEATS_DOORS", 1),
|
||||
("DOOR_OPEN_FR", "SEATS_DOORS", 1),
|
||||
("DOOR_OPEN_RL", "SEATS_DOORS", 1),
|
||||
("DOOR_OPEN_RR", "SEATS_DOORS", 1),
|
||||
("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 1),
|
||||
("TC_DISABLED", "ESP_CONTROL", 1),
|
||||
("STEER_FRACTION", "STEER_ANGLE_SENSOR", 0),
|
||||
("STEER_RATE", "STEER_ANGLE_SENSOR", 0),
|
||||
("CRUISE_ACTIVE", "PCM_CRUISE", 0),
|
||||
("CRUISE_STATE", "PCM_CRUISE", 0),
|
||||
("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
|
||||
("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0),
|
||||
("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers
|
||||
("LKA_STATE", "EPS_STATUS", 0),
|
||||
("IPAS_STATE", "EPS_STATUS", 1),
|
||||
("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0),
|
||||
]
|
||||
|
||||
checks = [
|
||||
("WHEEL_SPEEDS", 80),
|
||||
("STEER_ANGLE_SENSOR", 80),
|
||||
("PCM_CRUISE", 33),
|
||||
("STEER_TORQUE_SENSOR", 50),
|
||||
("EPS_STATUS", 25),
|
||||
]
|
||||
|
||||
if CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
|
||||
signals.append(("GAS_PEDAL", "GAS_PEDAL_ALT", 0))
|
||||
signals.append(("MAIN_ON", "PCM_CRUISE_ALT", 0))
|
||||
signals.append(("SET_SPEED", "PCM_CRUISE_ALT", 0))
|
||||
signals.append(("AUTO_HIGH_BEAM", "LIGHT_STALK_ISH", 0))
|
||||
checks += [
|
||||
("BRAKE_MODULE", 50),
|
||||
("GAS_PEDAL_ALT", 50),
|
||||
("PCM_CRUISE_ALT", 1),
|
||||
]
|
||||
else:
|
||||
signals += [
|
||||
("AUTO_HIGH_BEAM", "LIGHT_STALK", 0),
|
||||
("GAS_PEDAL", "GAS_PEDAL", 0),
|
||||
]
|
||||
checks += [
|
||||
("BRAKE_MODULE", 40),
|
||||
("GAS_PEDAL", 33),
|
||||
]
|
||||
|
||||
if CP.carFingerprint == CAR.LEXUS_IS:
|
||||
signals.append(("MAIN_ON", "DSU_CRUISE", 0))
|
||||
signals.append(("SET_SPEED", "DSU_CRUISE", 0))
|
||||
checks.append(("DSU_CRUISE", 5))
|
||||
else:
|
||||
signals.append(("MAIN_ON", "PCM_CRUISE_2", 0))
|
||||
signals.append(("SET_SPEED", "PCM_CRUISE_2", 0))
|
||||
signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0))
|
||||
checks.append(("PCM_CRUISE_2", 33))
|
||||
|
||||
if CP.carFingerprint in NO_DSU_CAR or CP.carFingerprint == CAR.LEXUS_ISH:
|
||||
signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)]
|
||||
|
||||
if CP.carFingerprint == CAR.PRIUS:
|
||||
signals += [("STATE", "AUTOPARK_STATUS", 0)]
|
||||
|
||||
# add gas interceptor reading if we are using it
|
||||
if CP.enableGasInterceptor:
|
||||
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
|
||||
signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0))
|
||||
checks.append(("GAS_SENSOR", 50))
|
||||
|
||||
checks = []
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
|
||||
|
||||
def get_cam_can_parser(CP):
|
||||
|
||||
signals = [("FORCE", "PRE_COLLISION", 0), ("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0)]
|
||||
|
||||
# use steering message to check if panda is connected to frc
|
||||
checks = [("STEERING_LKA", 42)]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
|
||||
|
||||
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']
|
||||
self.angle_offset = 0.
|
||||
self.init_angle_offset = False
|
||||
|
||||
# dragonpilot
|
||||
self.dragon_toyota_stock_dsu = False
|
||||
self.ts_last_check = 0.
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
# dragonpilot, don't check for param too often as it's a kernel call
|
||||
ts = sec_since_boot()
|
||||
if ts - self.ts_last_check > 5.:
|
||||
self.dragon_toyota_stock_dsu = True if params.get("DragonToyotaStockDSU", encoding='utf8') == "1" else False
|
||||
self.ts_last_check = ts
|
||||
|
||||
# update prevs, update must run once per loop
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
|
||||
self.door_all_closed = not 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']])
|
||||
self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED']
|
||||
|
||||
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
|
||||
if self.CP.enableGasInterceptor:
|
||||
self.pedal_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
|
||||
elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
|
||||
self.pedal_gas = cp.vl["GAS_PEDAL_ALT"]['GAS_PEDAL']
|
||||
else:
|
||||
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
|
||||
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']
|
||||
|
||||
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
|
||||
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
|
||||
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
|
||||
|
||||
if self.CP.carFingerprint in TSS2_CAR:
|
||||
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
|
||||
elif self.CP.carFingerprint in NO_DSU_CAR or self.CP.carFingerprint == CAR.LEXUS_ISH:
|
||||
# cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] 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.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
|
||||
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
|
||||
if abs(angle_wheel) > 1e-3 and abs(self.angle_steers) > 1e-3 and not self.init_angle_offset:
|
||||
self.init_angle_offset = True
|
||||
self.angle_offset = self.angle_steers - angle_wheel
|
||||
else:
|
||||
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 = 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']
|
||||
elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
|
||||
self.main_on = cp.vl["PCM_CRUISE_ALT"]['MAIN_ON']
|
||||
else:
|
||||
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
|
||||
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
|
||||
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
|
||||
|
||||
# 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_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
|
||||
self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
|
||||
self.brake_error = 0
|
||||
self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
|
||||
self.steer_torque_motor = 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
|
||||
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
|
||||
|
||||
self.user_brake = 0
|
||||
if self.CP.carFingerprint == CAR.LEXUS_IS:
|
||||
self.v_cruise_pcm = cp.vl["DSU_CRUISE"]['SET_SPEED']
|
||||
self.low_speed_lockout = False
|
||||
elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
|
||||
self.v_cruise_pcm = cp.vl["PCM_CRUISE_ALT"]['SET_SPEED']
|
||||
self.low_speed_lockout = False
|
||||
else:
|
||||
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
|
||||
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
|
||||
if self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
|
||||
# Lexus ISH does not have curise status value (always 0), so we use curise_active value instead
|
||||
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']
|
||||
else:
|
||||
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
|
||||
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
|
||||
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)
|
||||
if self.CP.carFingerprint == CAR.PRIUS:
|
||||
self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
|
||||
elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
|
||||
self.generic_toggle = bool(cp.vl["LIGHT_STALK_ISH"]['AUTO_HIGH_BEAM'])
|
||||
else:
|
||||
self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
|
||||
|
||||
self.stock_aeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
|
||||
|
||||
if self.dragon_toyota_stock_dsu and self.generic_toggle and self.main_on:
|
||||
enable_acc = True
|
||||
if not self.gear_shifter == GearShifter.drive or not self.seatbelt or not self.door_all_closed:
|
||||
enable_acc = False
|
||||
self.pcm_acc_active = enable_acc
|
||||
if self.standstill:
|
||||
self.pcm_acc_status = 7
|
||||
else:
|
||||
self.pcm_acc_status = 1
|
||||
from common.numpy_fast import mean
|
||||
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
|
||||
from common.realtime import sec_since_boot
|
||||
from common.params import Params
|
||||
params = Params()
|
||||
|
||||
|
||||
def get_can_parser(CP):
|
||||
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0),
|
||||
("GEAR", "GEAR_PACKET", 0),
|
||||
("BRAKE_PRESSED", "BRAKE_MODULE", 0),
|
||||
("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0),
|
||||
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
|
||||
("DOOR_OPEN_FL", "SEATS_DOORS", 1),
|
||||
("DOOR_OPEN_FR", "SEATS_DOORS", 1),
|
||||
("DOOR_OPEN_RL", "SEATS_DOORS", 1),
|
||||
("DOOR_OPEN_RR", "SEATS_DOORS", 1),
|
||||
("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 1),
|
||||
("TC_DISABLED", "ESP_CONTROL", 1),
|
||||
("STEER_FRACTION", "STEER_ANGLE_SENSOR", 0),
|
||||
("STEER_RATE", "STEER_ANGLE_SENSOR", 0),
|
||||
("CRUISE_ACTIVE", "PCM_CRUISE", 0),
|
||||
("CRUISE_STATE", "PCM_CRUISE", 0),
|
||||
("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
|
||||
("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0),
|
||||
("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers
|
||||
("LKA_STATE", "EPS_STATUS", 0),
|
||||
("IPAS_STATE", "EPS_STATUS", 1),
|
||||
("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0),
|
||||
]
|
||||
|
||||
checks = [
|
||||
("WHEEL_SPEEDS", 80),
|
||||
("STEER_ANGLE_SENSOR", 80),
|
||||
("PCM_CRUISE", 33),
|
||||
("STEER_TORQUE_SENSOR", 50),
|
||||
("EPS_STATUS", 25),
|
||||
]
|
||||
|
||||
if CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
|
||||
signals.append(("GAS_PEDAL", "GAS_PEDAL_ALT", 0))
|
||||
signals.append(("MAIN_ON", "PCM_CRUISE_ALT", 0))
|
||||
signals.append(("SET_SPEED", "PCM_CRUISE_ALT", 0))
|
||||
signals.append(("AUTO_HIGH_BEAM", "LIGHT_STALK_ISH", 0))
|
||||
checks += [
|
||||
("BRAKE_MODULE", 50),
|
||||
("GAS_PEDAL_ALT", 50),
|
||||
("PCM_CRUISE_ALT", 1),
|
||||
]
|
||||
else:
|
||||
signals += [
|
||||
("AUTO_HIGH_BEAM", "LIGHT_STALK", 0),
|
||||
("GAS_PEDAL", "GAS_PEDAL", 0),
|
||||
]
|
||||
checks += [
|
||||
("BRAKE_MODULE", 40),
|
||||
("GAS_PEDAL", 33),
|
||||
]
|
||||
|
||||
if CP.carFingerprint == CAR.LEXUS_IS:
|
||||
signals.append(("MAIN_ON", "DSU_CRUISE", 0))
|
||||
signals.append(("SET_SPEED", "DSU_CRUISE", 0))
|
||||
checks.append(("DSU_CRUISE", 5))
|
||||
else:
|
||||
signals.append(("MAIN_ON", "PCM_CRUISE_2", 0))
|
||||
signals.append(("SET_SPEED", "PCM_CRUISE_2", 0))
|
||||
signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0))
|
||||
checks.append(("PCM_CRUISE_2", 33))
|
||||
|
||||
if CP.carFingerprint in NO_DSU_CAR or CP.carFingerprint == CAR.LEXUS_ISH:
|
||||
signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)]
|
||||
|
||||
if CP.carFingerprint == CAR.PRIUS:
|
||||
signals += [("STATE", "AUTOPARK_STATUS", 0)]
|
||||
|
||||
# add gas interceptor reading if we are using it
|
||||
if CP.enableGasInterceptor:
|
||||
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
|
||||
signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0))
|
||||
checks.append(("GAS_SENSOR", 50))
|
||||
|
||||
checks = []
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
|
||||
|
||||
def get_cam_can_parser(CP):
|
||||
|
||||
signals = [("FORCE", "PRE_COLLISION", 0), ("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0)]
|
||||
|
||||
# use steering message to check if panda is connected to frc
|
||||
checks = [("STEERING_LKA", 42)]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
|
||||
|
||||
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']
|
||||
self.angle_offset = 0.
|
||||
self.init_angle_offset = False
|
||||
|
||||
# dragonpilot
|
||||
self.dragon_toyota_stock_dsu = False
|
||||
self.ts_last_check = 0.
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
# dragonpilot, don't check for param too often as it's a kernel call
|
||||
ts = sec_since_boot()
|
||||
if ts - self.ts_last_check > 5.:
|
||||
self.dragon_toyota_stock_dsu = True if params.get("DragonToyotaStockDSU", encoding='utf8') == "1" else False
|
||||
self.ts_last_check = ts
|
||||
|
||||
# update prevs, update must run once per loop
|
||||
self.prev_left_blinker_on = self.left_blinker_on
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
|
||||
self.door_all_closed = not 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']])
|
||||
self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED']
|
||||
|
||||
self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED']
|
||||
if self.CP.enableGasInterceptor:
|
||||
self.pedal_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2.
|
||||
elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
|
||||
self.pedal_gas = cp.vl["GAS_PEDAL_ALT"]['GAS_PEDAL']
|
||||
else:
|
||||
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
|
||||
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED']
|
||||
|
||||
self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS
|
||||
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
|
||||
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
|
||||
|
||||
if self.CP.carFingerprint in TSS2_CAR:
|
||||
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
|
||||
elif self.CP.carFingerprint in NO_DSU_CAR or self.CP.carFingerprint == CAR.LEXUS_ISH:
|
||||
# cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] 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.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
|
||||
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
|
||||
if abs(angle_wheel) > 1e-3 and abs(self.angle_steers) > 1e-3 and not self.init_angle_offset:
|
||||
self.init_angle_offset = True
|
||||
self.angle_offset = self.angle_steers - angle_wheel
|
||||
else:
|
||||
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 = 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']
|
||||
elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
|
||||
self.main_on = cp.vl["PCM_CRUISE_ALT"]['MAIN_ON']
|
||||
else:
|
||||
self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON']
|
||||
self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
|
||||
self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
|
||||
|
||||
# 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_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5]
|
||||
self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3
|
||||
self.brake_error = 0
|
||||
self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER']
|
||||
self.steer_torque_motor = 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
|
||||
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD
|
||||
|
||||
self.user_brake = 0
|
||||
if self.CP.carFingerprint == CAR.LEXUS_IS:
|
||||
self.v_cruise_pcm = cp.vl["DSU_CRUISE"]['SET_SPEED']
|
||||
self.low_speed_lockout = False
|
||||
elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
|
||||
self.v_cruise_pcm = cp.vl["PCM_CRUISE_ALT"]['SET_SPEED']
|
||||
self.low_speed_lockout = False
|
||||
else:
|
||||
self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED']
|
||||
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
|
||||
if self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
|
||||
# Lexus ISH does not have curise status value (always 0), so we use curise_active value instead
|
||||
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']
|
||||
else:
|
||||
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
|
||||
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
|
||||
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)
|
||||
if self.CP.carFingerprint == CAR.PRIUS:
|
||||
self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
|
||||
elif self.CP.carFingerprint in [CAR.LEXUS_ISH, CAR.LEXUS_GSH]:
|
||||
self.generic_toggle = bool(cp.vl["LIGHT_STALK_ISH"]['AUTO_HIGH_BEAM'])
|
||||
else:
|
||||
self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
|
||||
|
||||
self.stock_aeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
|
||||
|
||||
if self.dragon_toyota_stock_dsu and self.generic_toggle and self.main_on:
|
||||
enable_acc = True
|
||||
if not self.seatbelt or not self.door_all_closed:
|
||||
enable_acc = False
|
||||
self.pcm_acc_active = enable_acc
|
||||
if self.standstill:
|
||||
self.pcm_acc_status = 7
|
||||
else:
|
||||
self.pcm_acc_status = 1
|
||||
|
||||
Reference in New Issue
Block a user