191 lines
7.3 KiB
Python
191 lines
7.3 KiB
Python
from opendbc.can.can_define import CANDefine
|
||
from opendbc.can.parser import CANParser
|
||
from opendbc.car import Bus, create_button_events, structs
|
||
from opendbc.car.common.conversions import Conversions as CV
|
||
from opendbc.car.interfaces import CarStateBase
|
||
from opendbc.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags, Buttons
|
||
|
||
ButtonType = structs.CarState.ButtonEvent.Type
|
||
BUTTONS_DICT = {Buttons.SET_PLUS: ButtonType.accelCruise, Buttons.SET_MINUS: ButtonType.decelCruise,
|
||
Buttons.RESUME: ButtonType.resumeCruise, Buttons.CANCEL: ButtonType.cancel}
|
||
|
||
class CarState(CarStateBase):
|
||
def __init__(self, CP):
|
||
super().__init__(CP)
|
||
|
||
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
|
||
self.shifter_values = can_define.dv["GEAR"]["GEAR"]
|
||
|
||
self.crz_btns_counter = 0
|
||
self.acc_active_last = False
|
||
self.low_speed_alert = False
|
||
self.lkas_allowed_speed = False
|
||
self.lkas_disabled = False
|
||
|
||
self.prev_distance_button = 0
|
||
self.distance_button = 0
|
||
self.pcmCruiseGap = 0 # copy from Hyundai
|
||
|
||
def update(self, can_parsers) -> structs.CarState:
|
||
cp = can_parsers[Bus.pt]
|
||
cp_cam = can_parsers[Bus.cam]
|
||
|
||
ret = structs.CarState()
|
||
|
||
self.prev_distance_button = self.distance_button
|
||
self.distance_button = cp.vl["CRZ_BTNS"]["DISTANCE_LESS"]
|
||
|
||
self.prev_cruise_buttons = self.cruise_buttons
|
||
|
||
if bool(cp.vl["CRZ_BTNS"]["SET_P"]):
|
||
self.cruise_buttons = Buttons.SET_PLUS
|
||
elif bool(cp.vl["CRZ_BTNS"]["SET_M"]):
|
||
self.cruise_buttons = Buttons.SET_MINUS
|
||
elif bool(cp.vl["CRZ_BTNS"]["RES"]):
|
||
self.cruise_buttons = Buttons.RESUME
|
||
else:
|
||
self.cruise_buttons = Buttons.NONE
|
||
|
||
ret.wheelSpeeds = self.get_wheel_speeds(
|
||
cp.vl["WHEEL_SPEEDS"]["FL"],
|
||
cp.vl["WHEEL_SPEEDS"]["FR"],
|
||
cp.vl["WHEEL_SPEEDS"]["RL"],
|
||
cp.vl["WHEEL_SPEEDS"]["RR"],
|
||
)
|
||
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"]
|
||
ret.standstill = speed_kph <= .1
|
||
|
||
can_gear = int(cp.vl["GEAR"]["GEAR"])
|
||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
|
||
ret.gearStep = cp.vl["GEAR"]["GEAR_BOX"]
|
||
ret.engineRpm = cp.vl["ENGINE_DATA"]["RPM"] # for mazda RPM
|
||
# 将CAN总线上的DISTANCE_SETTING值转换为与车辆显示一致的值
|
||
can_distance_setting = cp.vl["CRZ_CTRL"]["DISTANCE_SETTING"]
|
||
# 假设最大值为4，使用5减去CAN值来获取正确的显示值
|
||
ret.pcmCruiseGap = 5 - can_distance_setting if 1 <= can_distance_setting <= 4 else can_distance_setting
|
||
|
||
ret.genericToggle = bool(cp.vl["BLINK_INFO"]["HIGH_BEAMS"])
|
||
ret.leftBlindspot = cp.vl["BSM"]["LEFT_BS_STATUS"] != 0
|
||
ret.rightBlindspot = cp.vl["BSM"]["RIGHT_BS_STATUS"] != 0
|
||
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(40, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1,
|
||
cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1)
|
||
|
||
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"]
|
||
|
||
# TODO: this should be from 0 - 1.
|
||
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"]])
|
||
|
||
# TODO: this should be from 0 - 1.
|
||
ret.gas = cp.vl["ENGINE_DATA"]["PEDAL_GAS"]
|
||
ret.gasPressed = ret.gas > 0
|
||
|
||
# Either due to low speed or hands off
|
||
lkas_blocked = cp.vl["STEER_RATE"]["LKAS_BLOCK"] == 1
|
||
|
||
if self.CP.minSteerSpeed > 0:
|
||
# LKAS is enabled at 52kph going up and disabled at 45kph going down
|
||
# wait for LKAS_BLOCK signal to clear when going up since it lags behind the speed sometimes
|
||
if speed_kph > LKAS_LIMITS.ENABLE_SPEED and not lkas_blocked:
|
||
self.lkas_allowed_speed = True
|
||
elif speed_kph < LKAS_LIMITS.DISABLE_SPEED:
|
||
self.lkas_allowed_speed = False
|
||
else:
|
||
self.lkas_allowed_speed = True
|
||
|
||
# TODO: the signal used for available seems to be the adaptive cruise signal, instead of the main on
|
||
# it should be used for carState.cruiseState.nonAdaptive instead
|
||
ret.cruiseState.available = cp.vl["CRZ_CTRL"]["CRZ_AVAILABLE"] == 1
|
||
ret.cruiseState.enabled = cp.vl["CRZ_CTRL"]["CRZ_ACTIVE"] == 1
|
||
ret.cruiseState.standstill = cp.vl["PEDALS"]["STANDSTILL"] == 1
|
||
ret.cruiseState.speed = cp.vl["CRZ_EVENTS"]["CRZ_SPEED"] * CV.KPH_TO_MS
|
||
|
||
# stock lkas should be on
|
||
# TODO: is this needed?
|
||
ret.invalidLkasSetting = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0
|
||
|
||
if ret.cruiseState.enabled:
|
||
if not self.lkas_allowed_speed and self.acc_active_last:
|
||
self.low_speed_alert = True
|
||
else:
|
||
self.low_speed_alert = False
|
||
ret.lowSpeedAlert = self.low_speed_alert
|
||
|
||
# Check if LKAS is disabled due to lack of driver torque when all other states indicate
|
||
# it should be enabled (steer lockout). Don't warn until we actually get lkas active
|
||
# and lose it again, i.e, after initial lkas activation
|
||
ret.steerFaultTemporary = self.lkas_allowed_speed and lkas_blocked
|
||
|
||
self.acc_active_last = ret.cruiseState.enabled
|
||
|
||
self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"]
|
||
|
||
# camera signals
|
||
self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0
|
||
self.cam_lkas = cp_cam.vl["CAM_LKAS"]
|
||
self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"]
|
||
ret.steerFaultPermanent = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1
|
||
|
||
self.lkas_previously_enabled = self.lkas_enabled
|
||
self.lkas_enabled = not self.lkas_disabled
|
||
|
||
# TODO: add button types for inc and dec
|
||
#ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise})
|
||
ret.buttonEvents = [
|
||
*create_button_events(self.cruise_buttons, self.prev_cruise_buttons, BUTTONS_DICT),
|
||
*create_button_events(self.distance_button, self.prev_distance_button, {1: ButtonType.gapAdjustCruise}),
|
||
*create_button_events(self.lkas_enabled, self.lkas_previously_enabled, {1: ButtonType.lfaButton}),
|
||
]
|
||
return ret
|
||
|
||
@staticmethod
|
||
def get_can_parsers(CP):
|
||
pt_messages = [
|
||
# sig_address, frequency
|
||
("BLINK_INFO", 10),
|
||
("STEER", 67),
|
||
("STEER_RATE", 83),
|
||
("STEER_TORQUE", 83),
|
||
("WHEEL_SPEEDS", 100),
|
||
]
|
||
|
||
if CP.flags & MazdaFlags.GEN1:
|
||
pt_messages += [
|
||
("ENGINE_DATA", 100),
|
||
("CRZ_CTRL", 50),
|
||
("CRZ_EVENTS", 50),
|
||
("CRZ_BTNS", 10),
|
||
("PEDALS", 50),
|
||
("BRAKE", 50),
|
||
("SEATBELT", 10),
|
||
("DOORS", 10),
|
||
("GEAR", 20),
|
||
("BSM", 10),
|
||
]
|
||
|
||
cam_messages = []
|
||
if CP.flags & MazdaFlags.GEN1:
|
||
cam_messages += [
|
||
# sig_address, frequency
|
||
("CAM_LANEINFO", 2),
|
||
("CAM_LKAS", 16),
|
||
]
|
||
|
||
return {
|
||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
|
||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, 2),
|
||
}
|