Add SDGM support
Co-Authored-By: Eric Brown <13560103+nworb-cire@users.noreply.github.com>
This commit is contained in:
FrogAi
@@ -5,7 +5,7 @@ from openpilot.common.realtime import DT_CTRL
|
||||
from opendbc.can.packer import CANPacker
|
||||
from openpilot.selfdrive.car import apply_driver_steer_torque_limits
|
||||
from openpilot.selfdrive.car.gm import gmcan
|
||||
from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR
|
||||
from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons, GMFlags, CC_ONLY_CAR, SDGM_CAR
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
NetworkLocation = car.CarParams.NetworkLocation
|
||||
@@ -152,7 +152,10 @@ class CarController:
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL > 0.04:
|
||||
if self.cancel_counter > CAMERA_CANCEL_DELAY_FRAMES:
|
||||
self.last_button_frame = self.frame
|
||||
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL))
|
||||
if self.CP.carFingerprint in SDGM_CAR:
|
||||
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, CS.buttons_counter, CruiseButtons.CANCEL))
|
||||
else:
|
||||
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL))
|
||||
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
# Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1
|
||||
|
||||
@@ -5,7 +5,7 @@ from openpilot.common.numpy_fast import mean
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from opendbc.can.parser import CANParser
|
||||
from openpilot.selfdrive.car.interfaces import CarStateBase
|
||||
from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD, GMFlags, CC_ONLY_CAR
|
||||
from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD, GMFlags, CC_ONLY_CAR, SDGM_CAR
|
||||
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
NetworkLocation = car.CarParams.NetworkLocation
|
||||
@@ -30,8 +30,12 @@ class CarState(CarStateBase):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
self.prev_cruise_buttons = self.cruise_buttons
|
||||
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
|
||||
self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"]
|
||||
if self.CP.carFingerprint not in SDGM_CAR:
|
||||
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
|
||||
self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"]
|
||||
else:
|
||||
self.cruise_buttons = cam_cp.vl["ASCMSteeringButton"]["ACCButtons"]
|
||||
self.buttons_counter = cam_cp.vl["ASCMSteeringButton"]["RollingCounter"]
|
||||
self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"])
|
||||
self.moving_backward = pt_cp.vl["EBCMWheelSpdRear"]["MovingBackward"] != 0
|
||||
|
||||
@@ -91,18 +95,32 @@ class CarState(CarStateBase):
|
||||
ret.steerFaultTemporary = self.lkas_status == 2
|
||||
ret.steerFaultPermanent = self.lkas_status == 3
|
||||
|
||||
# 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)
|
||||
if self.CP.carFingerprint not in SDGM_CAR:
|
||||
# 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
|
||||
# 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.parkingBrake = pt_cp.vl["BCMGeneralPlatformStatus"]["ParkBrakeSwActive"] == 1
|
||||
ret.parkingBrake = pt_cp.vl["BCMGeneralPlatformStatus"]["ParkBrakeSwActive"] == 1
|
||||
else:
|
||||
# 1 - open, 0 - closed
|
||||
ret.doorOpen = (cam_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or
|
||||
cam_cp.vl["BCMDoorBeltStatus"]["FrontRightDoor"] == 1 or
|
||||
cam_cp.vl["BCMDoorBeltStatus"]["RearLeftDoor"] == 1 or
|
||||
cam_cp.vl["BCMDoorBeltStatus"]["RearRightDoor"] == 1)
|
||||
|
||||
# 1 - latched
|
||||
ret.seatbeltUnlatched = cam_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0
|
||||
ret.leftBlinker = cam_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
|
||||
ret.rightBlinker = cam_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
|
||||
|
||||
ret.parkingBrake = cam_cp.vl["BCMGeneralPlatformStatus"]["ParkBrakeSwActive"] == 1
|
||||
ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
|
||||
ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
|
||||
ret.accFaulted = (pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED or
|
||||
@@ -113,7 +131,10 @@ class CarState(CarStateBase):
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
if self.CP.carFingerprint not in CC_ONLY_CAR:
|
||||
ret.cruiseState.speed = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCSpeedSetpoint"] * CV.KPH_TO_MS
|
||||
ret.stockAeb = cam_cp.vl["AEBCmd"]["AEBCmdActive"] != 0
|
||||
if self.CP.carFingerprint not in SDGM_CAR:
|
||||
ret.stockAeb = cam_cp.vl["AEBCmd"]["AEBCmdActive"] != 0
|
||||
else:
|
||||
ret.stockAeb = False
|
||||
# openpilot controls nonAdaptive when not pcmCruise
|
||||
if self.CP.pcmCruise:
|
||||
ret.cruiseState.nonAdaptive = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCCruiseState"] not in (2, 3)
|
||||
@@ -129,9 +150,19 @@ class CarState(CarStateBase):
|
||||
messages = []
|
||||
if CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
messages += [
|
||||
("AEBCmd", 10),
|
||||
("ASCMLKASteeringCmd", 10),
|
||||
]
|
||||
if CP.carFingerprint in SDGM_CAR:
|
||||
messages += [
|
||||
("BCMTurnSignals", 1),
|
||||
("BCMDoorBeltStatus", 10),
|
||||
("BCMGeneralPlatformStatus", 10),
|
||||
("ASCMSteeringButton", 33),
|
||||
]
|
||||
else:
|
||||
messages += [
|
||||
("AEBCmd", 10),
|
||||
]
|
||||
if CP.carFingerprint not in CC_ONLY_CAR:
|
||||
messages += [
|
||||
("ASCMActiveCruiseControlStatus", 25),
|
||||
@@ -142,22 +173,36 @@ class CarState(CarStateBase):
|
||||
@staticmethod
|
||||
def get_can_parser(CP):
|
||||
messages = [
|
||||
("BCMTurnSignals", 1),
|
||||
("ECMPRDNL2", 10),
|
||||
("PSCMStatus", 10),
|
||||
("ESPStatus", 10),
|
||||
("BCMDoorBeltStatus", 10),
|
||||
("BCMGeneralPlatformStatus", 10),
|
||||
("EBCMWheelSpdFront", 20),
|
||||
("EBCMWheelSpdRear", 20),
|
||||
("EBCMFrictionBrakeStatus", 20),
|
||||
("AcceleratorPedal2", 33),
|
||||
("ASCMSteeringButton", 33),
|
||||
("ECMEngineStatus", 100),
|
||||
("PSCMSteeringAngle", 100),
|
||||
("ECMAcceleratorPos", 80),
|
||||
]
|
||||
|
||||
# BSM does not send a signal until the first instance of it lighting up
|
||||
messages.append(("left_blindspot", 0))
|
||||
messages.append(("right_blindspot", 0))
|
||||
|
||||
if CP.carFingerprint in SDGM_CAR:
|
||||
messages += [
|
||||
("ECMPRDNL2", 40),
|
||||
("AcceleratorPedal2", 40),
|
||||
("ECMEngineStatus", 80),
|
||||
]
|
||||
else:
|
||||
messages += [
|
||||
("ECMPRDNL2", 10),
|
||||
("AcceleratorPedal2", 33),
|
||||
("ECMEngineStatus", 100),
|
||||
("BCMTurnSignals", 1),
|
||||
("BCMDoorBeltStatus", 10),
|
||||
("BCMGeneralPlatformStatus", 10),
|
||||
("ASCMSteeringButton", 33),
|
||||
]
|
||||
|
||||
# Used to read back last counter sent to PT by camera
|
||||
if CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
messages += [
|
||||
|
||||
@@ -167,4 +167,9 @@ FINGERPRINTS = {
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 717: 5, 723: 4, 730: 4, 761: 7, 800: 6, 840: 5, 842: 5, 844: 8, 869: 4, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 6, 1017: 8, 1020: 8, 1037: 5, 1105: 5, 1187: 5, 1195: 3, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1273: 3, 1276: 2, 1277: 7, 1278: 4, 1279: 4, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7
|
||||
}],
|
||||
CAR.XT4: [
|
||||
# Cadillac XT4 w/ ACC 2023
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 353: 3, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 761: 7, 806: 1, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 872: 1, 880: 6, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 5, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1037: 5, 1105: 5, 1187: 5, 1195: 3, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1273: 3, 1276: 2, 1277: 7, 1278: 4, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1417: 8, 1512: 8, 1517: 8, 1601: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1793: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1920: 8, 1924: 8, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1984: 8, 1988: 8, 2000: 8, 2001: 8, 2002: 8, 2016: 8, 2017: 8, 2018: 8, 2020: 8, 2021: 8, 2024: 8, 2026: 8
|
||||
}],
|
||||
}
|
||||
|
||||
@@ -7,7 +7,7 @@ from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.car import create_button_events, get_safety_config
|
||||
from openpilot.selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG
|
||||
from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus, GMFlags, CC_ONLY_CAR, SLOW_ACC
|
||||
from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus, GMFlags, CC_ONLY_CAR, SDGM_CAR, SLOW_ACC
|
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import get_friction
|
||||
|
||||
@@ -120,6 +120,15 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_CAM_LONG
|
||||
|
||||
elif candidate in SDGM_CAR:
|
||||
ret.experimentalLongitudinalAvailable = False
|
||||
ret.networkLocation = NetworkLocation.fwdCamera
|
||||
ret.pcmCruise = True
|
||||
ret.radarUnavailable = True
|
||||
ret.minEnableSpeed = -1. # engage speed is decided by ASCM
|
||||
ret.minSteerSpeed = 30 * CV.MPH_TO_MS
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_GM_HW_SDGM
|
||||
|
||||
else: # ASCM, OBD-II harness
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.networkLocation = NetworkLocation.gateway
|
||||
@@ -287,6 +296,14 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.XT4:
|
||||
ret.mass = 3660. * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.78
|
||||
ret.steerRatio = 14.4
|
||||
ret.centerToFront = ret.wheelbase * 0.4
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.CT6_CC:
|
||||
ret.wheelbase = 3.11
|
||||
ret.mass = 5198. * CV.LB_TO_KG
|
||||
@@ -326,7 +343,7 @@ class CarInterface(CarInterfaceBase):
|
||||
# TODO: verify 17 Volt can enable for the first time at a stop and allow for all GMs
|
||||
below_min_enable_speed = ret.vEgo < self.CP.minEnableSpeed or self.CS.moving_backward
|
||||
if below_min_enable_speed and not (ret.standstill and ret.brake >= 20 and
|
||||
self.CP.networkLocation == NetworkLocation.fwdCamera):
|
||||
(self.CP.networkLocation == NetworkLocation.fwdCamera and not self.CP.carFingerprint in SDGM_CAR)):
|
||||
events.add(EventName.belowEngageSpeed)
|
||||
if ret.cruiseState.standstill:
|
||||
events.add(EventName.resumeRequired)
|
||||
|
||||
@@ -50,6 +50,12 @@ class CarControllerParams:
|
||||
if CP.carFingerprint in SLOW_ACC and Params().get_bool("GasRegenCmd"):
|
||||
self.MAX_GAS = 8650
|
||||
|
||||
elif CP.carFingerprint in SDGM_CAR:
|
||||
self.MAX_GAS = 3400
|
||||
self.MAX_ACC_REGEN = 1514
|
||||
self.INACTIVE_REGEN = 1554
|
||||
max_regen_acceleration = 0.
|
||||
|
||||
else:
|
||||
self.MAX_GAS = 7168 # Safety limit, not ACC max. Stock ACC >8192 from standstill.
|
||||
self.MAX_ACC_REGEN = 5500 # Max ACC regen is slightly less than max paddle regen
|
||||
@@ -90,6 +96,7 @@ class CAR(StrEnum):
|
||||
YUKON_CC = "GMC YUKON NO ACC"
|
||||
CT6_CC = "CADILLAC CT6 NO ACC"
|
||||
TRAILBLAZER_CC = "CHEVROLET TRAILBLAZER 2024 NO ACC"
|
||||
XT4 = "CADILLAC XT4 2023"
|
||||
|
||||
|
||||
class Footnote(Enum):
|
||||
@@ -141,6 +148,7 @@ CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = {
|
||||
CAR.YUKON_CC: GMCarInfo("GMC Yukon No ACC"),
|
||||
CAR.CT6_CC: GMCarInfo("Cadillac CT6 No ACC"),
|
||||
CAR.TRAILBLAZER_CC: GMCarInfo("Chevrolet Trailblazer 2024 No ACC"),
|
||||
CAR.XT4: GMCarInfo("Cadillac XT4 2023", "Driver Assist Package"),
|
||||
}
|
||||
|
||||
|
||||
@@ -182,6 +190,9 @@ CC_ONLY_CAR = {CAR.VOLT_CC, CAR.BOLT_CC, CAR.EQUINOX_CC, CAR.SUBURBAN_CC, CAR.YU
|
||||
# Slow acceleration cars
|
||||
SLOW_ACC = {CAR.SILVERADO}
|
||||
|
||||
# We're integrated at the Safety Data Gateway Module on these cars
|
||||
SDGM_CAR = {CAR.XT4}
|
||||
|
||||
# We're integrated at the camera with VOACC on these cars (instead of ASCM w/ OBD-II harness)
|
||||
CAMERA_ACC_CAR = {CAR.BOLT_EUV, CAR.SILVERADO, CAR.EQUINOX, CAR.TRAILBLAZER}
|
||||
CAMERA_ACC_CAR.update({CAR.VOLT_CC, CAR.BOLT_CC, CAR.EQUINOX_CC, CAR.YUKON_CC, CAR.CT6_CC, CAR.TRAILBLAZER_CC})
|
||||
|
||||
Reference in New Issue
Block a user