sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
commit
de77c89cf6
3995 files changed
+1260688
No files matched your search
Whitespace-only changes.
@@ -0,0 +1,41 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
|
||||
def crc8_pedal(data):
|
||||
crc = 0xFF # standard init value
|
||||
poly = 0xD5 # standard crc8: x8+x7+x6+x4+x2+1
|
||||
size = len(data)
|
||||
for i in range(size - 1, -1, -1):
|
||||
crc ^= data[i]
|
||||
for _ in range(8):
|
||||
if (crc & 0x80) != 0:
|
||||
crc = ((crc << 1) ^ poly) & 0xFF
|
||||
else:
|
||||
crc <<= 1
|
||||
return crc
|
||||
|
||||
|
||||
def create_gas_interceptor_command(packer, gas_amount, idx):
|
||||
# Common gas pedal msg generator
|
||||
enable = gas_amount > 0.001
|
||||
|
||||
values = {
|
||||
"ENABLE": enable,
|
||||
"PEDAL_COUNTER": idx & 0xF,
|
||||
}
|
||||
|
||||
if enable:
|
||||
values["GAS_COMMAND"] = gas_amount * 255.
|
||||
values["GAS_COMMAND2"] = gas_amount * 255.
|
||||
|
||||
dat = packer.make_can_msg("GAS_COMMAND", 0, values)[1]
|
||||
|
||||
checksum = crc8_pedal(dat[:-1])
|
||||
values["PEDAL_CHECKSUM"] = checksum
|
||||
|
||||
return packer.make_can_msg("GAS_COMMAND", 0, values)
|
||||
File diff suppressed because it is too large.
Load diff
Whitespace-only changes.
@@ -0,0 +1,23 @@
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.chrysler.values import RAM_DT
|
||||
from opendbc.sunnypilot.car.chrysler.values_ext import ChryslerFlagsSP
|
||||
|
||||
GearShifter = structs.CarState.GearShifter
|
||||
|
||||
|
||||
class CarControllerExt:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
def get_lkas_control_bit(self, CS: CarStateBase, CC: structs.CarControl, lkas_control_bit: bool) -> bool:
|
||||
if self.CP_SP.flags & ChryslerFlagsSP.NO_MIN_STEERING_SPEED:
|
||||
lkas_control_bit = CC.latActive
|
||||
elif self.CP.carFingerprint in RAM_DT:
|
||||
if self.CP.minEnableSpeed <= CS.out.vEgo <= self.CP.minEnableSpeed + 0.5:
|
||||
lkas_control_bit = True
|
||||
if self.CP.minEnableSpeed >= 14.5 and CS.out.gearShifter != GearShifter.drive:
|
||||
lkas_control_bit = False
|
||||
|
||||
return lkas_control_bit
|
||||
@@ -0,0 +1,40 @@
|
||||
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from enum import StrEnum
|
||||
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.car.chrysler.values import RAM_HD
|
||||
from opendbc.sunnypilot.car.chrysler.values_ext import BUTTONS
|
||||
|
||||
|
||||
class CarStateExt:
|
||||
def __init__(self, CP, CP_SP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
self.button_events = []
|
||||
self.button_states = {button.event_type: False for button in BUTTONS}
|
||||
|
||||
def update(self, ret: structs.CarState, ret_sp: structs.CarStateSP, can_parsers: dict[StrEnum, CANParser]):
|
||||
cp = can_parsers[Bus.pt]
|
||||
|
||||
button_events = []
|
||||
for button in BUTTONS:
|
||||
state = (cp.vl[button.can_addr][button.can_msg] in button.values)
|
||||
if self.button_states[button.event_type] != state:
|
||||
event = structs.CarState.ButtonEvent.new_message()
|
||||
event.type = button.event_type
|
||||
event.pressed = state
|
||||
button_events.append(event)
|
||||
self.button_states[button.event_type] = state
|
||||
self.button_events = button_events
|
||||
|
||||
if self.CP.carFingerprint in RAM_HD:
|
||||
ret.steeringAngleDeg = cp.vl["STEERING"]["STEERING_ANGLE"]
|
||||
@@ -0,0 +1,27 @@
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.chrysler.values import CAR
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
|
||||
FW_VERSIONS_EXT = {
|
||||
CAR.RAM_1500_5TH_GEN: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68453485AC',
|
||||
b'68510283AH',
|
||||
],
|
||||
(Ecu.eps, 0x75a, None): [
|
||||
b'68552791AA',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'05149390AA ',
|
||||
b'68378696AI ',
|
||||
b'68500631AF',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'68360085AF',
|
||||
b'68360086AL',
|
||||
b'68502996AC',
|
||||
],
|
||||
},
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.can_definitions import CanData
|
||||
from opendbc.car.chrysler import chryslercan
|
||||
from opendbc.car.chrysler.values import RAM_CARS
|
||||
from opendbc.sunnypilot.car.intelligent_cruise_button_management_interface_base import IntelligentCruiseButtonManagementInterfaceBase
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
SendButtonState = structs.IntelligentCruiseButtonManagement.SendButtonState
|
||||
|
||||
|
||||
class IntelligentCruiseButtonManagementInterface(IntelligentCruiseButtonManagementInterfaceBase):
|
||||
def __init__(self, CP, CP_SP):
|
||||
super().__init__(CP, CP_SP)
|
||||
|
||||
def update(self, CS, CC_SP, packer, frame, last_button_frame) -> list[CanData]:
|
||||
can_sends = []
|
||||
self.CC_SP = CC_SP
|
||||
self.ICBM = CC_SP.intelligentCruiseButtonManagement
|
||||
self.frame = frame
|
||||
self.last_button_frame = last_button_frame
|
||||
ram_cars = self.CP.carFingerprint in RAM_CARS
|
||||
das_bus = 2 if self.CP.carFingerprint in RAM_CARS else 0
|
||||
|
||||
if self.ICBM.sendButton != SendButtonState.none:
|
||||
accel = self.ICBM.sendButton == SendButtonState.increase
|
||||
decel = self.ICBM.sendButton == SendButtonState.decrease
|
||||
|
||||
if CS.button_counter != self.last_button_frame:
|
||||
self.last_button_frame = CS.button_counter
|
||||
|
||||
if ram_cars:
|
||||
can_sends.append(chryslercan.create_cruise_buttons(packer, CS.button_counter, das_bus,
|
||||
accel=accel, decel=decel))
|
||||
else:
|
||||
self.button_frame += 1
|
||||
button_counter_offset = [1, 1, 0, None][self.button_frame % 4]
|
||||
if button_counter_offset is not None:
|
||||
can_sends.append(chryslercan.create_cruise_buttons(packer, CS.button_counter + button_counter_offset, das_bus,
|
||||
accel=accel, decel=decel))
|
||||
|
||||
return can_sends
|
||||
@@ -0,0 +1,92 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from enum import StrEnum
|
||||
from collections import namedtuple
|
||||
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.chrysler.values import RAM_CARS
|
||||
|
||||
from opendbc.sunnypilot.mads_base import MadsCarStateBase
|
||||
from opendbc.can.parser import CANParser
|
||||
|
||||
MadsDataSP = namedtuple("MadsDataSP",
|
||||
["enable_mads", "paused", "lkas_disabled"])
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class MadsCarController:
|
||||
def __init__(self):
|
||||
self.mads = MadsDataSP(False, False, False)
|
||||
|
||||
@staticmethod
|
||||
def create_lkas_heartbit(packer, lkas_heartbit, mads):
|
||||
# LKAS_HEARTBIT (0x2D9) LKAS heartbeat
|
||||
values = {s: lkas_heartbit[s] for s in [
|
||||
"LKAS_DISABLED",
|
||||
"AUTO_HIGH_BEAM",
|
||||
"FORWARD_1",
|
||||
"FORWARD_2",
|
||||
"FORWARD_3",
|
||||
]}
|
||||
|
||||
if mads.enable_mads:
|
||||
values["LKAS_DISABLED"] = 1 if mads.lkas_disabled else 0
|
||||
|
||||
return packer.make_can_msg("LKAS_HEARTBIT", 0, values)
|
||||
|
||||
@staticmethod
|
||||
def mads_status_update(CC: structs.CarControl, CC_SP: structs.CarControlSP, CS) -> MadsDataSP:
|
||||
enable_mads = CC_SP.mads.available
|
||||
paused = CC_SP.mads.enabled and not CC.latActive
|
||||
|
||||
if any(be.type == ButtonType.lkas and be.pressed for be in CS.out.buttonEvents):
|
||||
CS.lkas_disabled = not CS.lkas_disabled
|
||||
|
||||
return MadsDataSP(enable_mads, paused, CS.lkas_disabled)
|
||||
|
||||
def update(self, CC: structs.CarControl, CC_SP: structs.CarControlSP, CS) -> None:
|
||||
self.mads = self.mads_status_update(CC, CC_SP, CS)
|
||||
|
||||
|
||||
class MadsCarState(MadsCarStateBase):
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
super().__init__(CP, CP_SP)
|
||||
self.lkas_heartbit = 0
|
||||
|
||||
self.init_lkas_disabled = False
|
||||
self.lkas_disabled = False
|
||||
|
||||
@staticmethod
|
||||
def get_parser(CP, pt_messages, cam_messages) -> None:
|
||||
if CP.carFingerprint in RAM_CARS:
|
||||
pt_messages += [
|
||||
("Center_Stack_2", 1),
|
||||
]
|
||||
else:
|
||||
pt_messages.append(("TRACTION_BUTTON", 1))
|
||||
cam_messages.append(("LKAS_HEARTBIT", 1))
|
||||
|
||||
def get_lkas_button(self, cp, cp_cam):
|
||||
if self.CP.carFingerprint in RAM_CARS:
|
||||
lkas_button = cp.vl["Center_Stack_2"]["LKAS_Button"]
|
||||
else:
|
||||
lkas_button = cp.vl["TRACTION_BUTTON"]["TOGGLE_LKAS"]
|
||||
self.lkas_heartbit = cp_cam.vl["LKAS_HEARTBIT"]
|
||||
if not self.init_lkas_disabled:
|
||||
self.lkas_disabled = cp_cam.vl["LKAS_HEARTBIT"]["LKAS_DISABLED"]
|
||||
self.init_lkas_disabled = True
|
||||
|
||||
return lkas_button
|
||||
|
||||
def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
|
||||
self.prev_lkas_button = self.lkas_button
|
||||
self.lkas_button = self.get_lkas_button(cp, cp_cam)
|
||||
@@ -0,0 +1,25 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from collections import namedtuple
|
||||
from enum import IntFlag
|
||||
|
||||
from opendbc.car import structs
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
|
||||
|
||||
BUTTONS = [
|
||||
Button(ButtonType.accelCruise, "CRUISE_BUTTONS", "ACC_Accel", [1]),
|
||||
Button(ButtonType.decelCruise, "CRUISE_BUTTONS", "ACC_Decel", [1]),
|
||||
Button(ButtonType.cancel, "CRUISE_BUTTONS", "ACC_Cancel", [1]),
|
||||
Button(ButtonType.resumeCruise, "CRUISE_BUTTONS", "ACC_Resume", [1]),
|
||||
]
|
||||
|
||||
|
||||
class ChryslerFlagsSP(IntFlag):
|
||||
NO_MIN_STEERING_SPEED = 1
|
||||
@@ -0,0 +1,39 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
|
||||
def merge_fw_versions(fw_versions, new_fw_versions):
|
||||
"""
|
||||
Merge firmware versions by extending lists for matching ECUs,
|
||||
adding all entries regardless of duplicates.
|
||||
"""
|
||||
for c, f in new_fw_versions.items():
|
||||
if c not in fw_versions:
|
||||
fw_versions[c] = f
|
||||
continue
|
||||
|
||||
for e, new_fw_list in f.items():
|
||||
if e not in fw_versions[c]:
|
||||
fw_versions[c][e] = new_fw_list
|
||||
else:
|
||||
fw_versions[c][e].extend(new_fw_list)
|
||||
|
||||
return fw_versions
|
||||
|
||||
|
||||
def merge_fingerprints(fingerprints, new_fingerprints):
|
||||
"""
|
||||
Merge fingerprints by extending lists for matching keys,
|
||||
adding all entries regardless of duplicates.
|
||||
"""
|
||||
for car, fp_list in new_fingerprints.items():
|
||||
if car not in fingerprints:
|
||||
fingerprints[car] = fp_list
|
||||
else:
|
||||
fingerprints[car].extend(fp_list)
|
||||
|
||||
return fingerprints
|
||||
Whitespace-only changes.
@@ -0,0 +1,24 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from enum import StrEnum
|
||||
|
||||
from opendbc.car import Bus,structs
|
||||
|
||||
from opendbc.sunnypilot.mads_base import MadsCarStateBase
|
||||
from opendbc.can.parser import CANParser
|
||||
|
||||
|
||||
class MadsCarState(MadsCarStateBase):
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
super().__init__(CP, CP_SP)
|
||||
|
||||
def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
cp = can_parsers[Bus.pt]
|
||||
|
||||
self.prev_lkas_button = self.lkas_button
|
||||
self.lkas_button = cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"]
|
||||
Whitespace-only changes.
@@ -0,0 +1,26 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from enum import StrEnum
|
||||
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.sunnypilot.car.gm.values_ext import GMFlagsSP
|
||||
|
||||
|
||||
class CarStateExt:
|
||||
def __init__(self, CP, CP_SP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
def update(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
pt_cp = can_parsers[Bus.pt]
|
||||
|
||||
if self.CP_SP.flags & GMFlagsSP.NON_ACC:
|
||||
ret.cruiseState.enabled = pt_cp.vl["ECMCruiseControl"]["CruiseActive"] != 0
|
||||
ret.cruiseState.speed = pt_cp.vl["ECMCruiseControl"]["CruiseSetSpeed"] * CV.KPH_TO_MS
|
||||
ret.accFaulted = False
|
||||
@@ -0,0 +1,122 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
# ruff: noqa: E501
|
||||
from opendbc.car.gm.values import CAR
|
||||
|
||||
FINGERPRINTS_EXT = {
|
||||
# FIXME-SP: Need user validation
|
||||
# CAR.CHEVROLET_VOLT: [{
|
||||
# 189: 7, 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 386: 8, 388: 8, 451: 8, 452: 8, 453: 6, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 497: 8, 500: 6, 501: 8, 513: 6, 528: 4, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 566: 5, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1922: 7
|
||||
# }],
|
||||
# FIXME-SP: Need a message to distinguish flashed from non-flashed
|
||||
# CAR.CHEVROLET_VOLT_CC: [
|
||||
# Volt Premier w/o acc 2016
|
||||
# {
|
||||
# 170: 8, 171: 8, 189: 7, 190: 6, 192: 5, 193: 8, 197: 8, 199: 4, 201: 6, 209: 7, 211: 2, 241: 6, 288: 5, 289: 1, 290: 1, 298: 2, 304: 8, 308: 4, 309: 8, 311: 8, 313: 8, 320: 8, 328: 1, 352: 5, 368: 8, 381: 6, 384: 8, 386: 5, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 8, 479: 3, 481: 7, 485: 8, 489: 5, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 3, 508: 8, 512: 3, 528: 4, 530: 8, 532: 6, 537: 4, 539: 8, 542: 7, 546: 7, 550: 8, 554: 3, 558: 8, 560: 6, 562: 8, 563: 5, 564: 5, 565: 8, 566: 5, 567: 3, 568: 1, 577: 8, 578: 8, 594: 8, 647: 3, 707: 8, 711: 6, 717: 5, 761: 7, 800: 6, 810: 8, 821: 4, 823: 7, 832: 8, 840: 5, 842: 6, 844: 8, 866: 4, 869: 4, 961: 8, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 5, 1003: 5, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1602: 8, 1618: 8, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1928: 7, 1930: 7, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2024: 8, 2025: 8, 2028: 8
|
||||
# },
|
||||
# {
|
||||
# 201: 8, 493: 8, 495: 4, 193: 8, 197: 8, 209: 7, 171: 8, 456: 8, 199: 4, 489: 8, 211: 2, 499: 3, 390: 7, 532: 6, 568: 1, 761: 7, 381: 6, 485: 8, 189: 7, 479: 3, 711: 6, 501: 8, 241: 6, 717: 5, 869: 4, 389: 2, 454: 8, 170: 8, 190: 6, 497: 8, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 500: 6, 508: 8, 528: 4, 647: 3, 1105: 6, 1005: 6, 481: 7, 844: 8, 866: 4, 564: 5, 969: 8, 388: 8, 352: 5, 562: 8, 961: 8, 386: 8, 707: 8, 977: 8, 979: 7, 298: 8, 840: 5, 842: 5, 988: 6, 1001: 8, 560: 8, 546: 7, 558: 8, 309: 8, 995: 7, 311: 8, 566: 5, 567:3, 989: 8, 384: 4, 800: 6, 1033: 7, 1034: 7, 313: 8, 554: 3, 810: 8, 1017: 8, 1019: 2, 1020: 8, 1217: 8, 1223: 3, 1233: 8, 1227: 4, 1417: 8, 1009: 8, 1221: 5, 1275: 3, 1225: 7, 289: 8, 550: 8, 1273: 3, 1928: 7, 1187: 4, 1265: 8, 1927: 7, 1267: 1, 1906: 7, 288: 5, 304: 1, 328: 1, 1912: 7, 320: 3, 1910: 7, 563: 5, 1249: 8, 1930: 7, 1257: 6, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 565: 5, 1280: 4, 1907: 7
|
||||
# },
|
||||
# # Volt Premier w/o ACC 2018 + Pedal
|
||||
# {
|
||||
# 189: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 451: 8, 452: 8, 453: 6, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 497: 8, 500: 6, 501: 8, 513: 6, 528: 4, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 566: 5, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 717: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1922: 7, 1930: 7
|
||||
# }
|
||||
# ],
|
||||
CAR.CHEVROLET_BOLT_NON_ACC: [
|
||||
# Bolt Premier w/o ACC 2017
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 192: 5, 193: 8, 197: 8, 201: 6, 209: 7, 211: 2, 241: 6, 289: 1, 290: 1, 298: 8, 304: 8, 309: 8, 311: 8, 313: 8, 320: 8, 322: 7, 328: 1, 352: 5, 353: 3, 368: 8, 381: 6, 384: 8, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 5, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 512: 3, 513: 6, 514: 2, 516: 4, 519: 2, 521: 3, 528: 5, 530: 8, 532: 7, 537: 5, 539: 8, 542: 7, 546: 7, 550: 8, 554: 3, 558: 8, 560: 6, 562: 4, 563: 5, 564: 5, 565: 8, 566: 6, 567: 5, 568: 1, 569: 3, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 832: 8, 840: 6, 842: 6, 844: 8, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 5, 1003: 5, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1601: 8, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1927: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8
|
||||
},
|
||||
# Bolt EV Premier 2017
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 192: 5, 193: 8, 197: 8, 201: 6, 209: 7, 211: 2, 241: 6, 289: 1, 290: 1, 298: 8, 304: 8, 309: 8, 311: 8, 313: 8, 320: 8, 322: 7, 328: 1, 352: 5, 353: 3, 368: 8, 381: 6, 384: 8, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 5, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 513: 6, 512: 3, 514: 2, 516: 4, 519: 2, 521: 3, 528: 5, 530: 8, 532: 7, 537: 5, 539: 8, 542: 7, 546: 7, 550: 8, 554: 3, 558: 8, 560: 6, 562: 4, 563: 5, 564: 5, 565: 8, 566: 6, 567: 5, 568: 1, 569: 3, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 832: 8, 840: 6, 842: 6, 844: 8, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 5, 1003: 5, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1601: 8, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1927: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8
|
||||
},
|
||||
# Bolt EV Premier 2017 w Pedal
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 192: 5, 193: 8, 197: 8, 201: 6, 209: 7, 211: 2, 241: 6, 289: 1, 290: 1, 298: 8, 304: 8, 309: 8, 311: 8, 313: 8, 320: 8, 322: 7, 328: 1, 352: 5, 353: 3, 368: 8, 381: 6, 384: 8, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 5, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 512: 3, 512: 6, 513: 6, 514: 2, 516: 4, 519: 2, 521: 3, 528: 5, 530: 8, 532: 7, 537: 5, 539: 8, 542: 7, 546: 7, 550: 8, 554: 3, 558: 8, 560: 6, 562: 4, 563: 5, 564: 5, 565: 8, 566: 6, 567: 5, 568: 1, 569: 3, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 832: 8, 840: 6, 842: 6, 844: 8, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 5, 1003: 5, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1601: 8, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1927: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8 # noqa: F601
|
||||
},
|
||||
# Bolt EV Premier 2017 2 w Pedal
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 6, 384: 4, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 513: 6, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 6, 567: 5, 568: 1, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1922: 7, 1927: 7
|
||||
},
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 6, 384: 4, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 513: 6, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 6, 567: 5, 568: 1, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1922: 7, 1927: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8
|
||||
}],
|
||||
CAR.CHEVROLET_BOLT_NON_ACC_1ST_GEN: [
|
||||
# Bolt Premier no ACC 2018 + Pedal
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 6, 384: 4, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 513: 6, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 6, 567: 5, 568: 1, 573: 1, 577: 8, 592: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1601: 8, 1616: 8, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1922: 7, 1927: 7, 2020: 8, 2023: 8, 2028: 8, 2031: 8
|
||||
},
|
||||
# Bolt Premier no ACC 2019 + Pedal
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 8, 384: 4, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 512: 6, 513: 6, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 7, 567: 5, 568: 2, 569: 3, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 975: 2, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1037: 5, 1105: 5, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1236: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1268: 2, 1275: 3, 1279: 4, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1927: 7, 2016: 8, 2024: 8
|
||||
},
|
||||
# Bolt Premier no ACC 2020
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 8, 384: 4, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 513: 6, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 7, 567: 5, 568: 2, 569: 3, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 975: 2, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1037: 5, 1105: 5, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1236: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1268: 2, 1275: 3, 1279: 4, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1927: 7, 2016: 8, 2024: 8
|
||||
}],
|
||||
CAR.CHEVROLET_BOLT_NON_ACC_2ND_GEN: [
|
||||
# Bolt EV Premier no ACC 2023 w Pedal
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 3, 241: 6, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 308: 4, 309: 8, 311: 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, 390: 7, 398: 8, 407: 7, 417: 8, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 513: 6, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 8, 567: 5, 568: 2, 569: 3, 573: 1, 577: 8, 592: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 711: 6, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 866: 4, 869: 4, 872: 1, 880: 6, 961: 8, 967: 4, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1010: 8, 1013: 6, 1015: 1, 1017: 8, 1019: 2, 1020: 8, 1037: 5, 1105: 5, 1187: 5, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1601: 8, 1616: 8, 1618: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1913: 7, 1922: 7, 1927: 7, 1930: 7, 2016: 8, 2020: 8, 2023: 8, 2024: 8, 2028: 8, 2031: 8
|
||||
},
|
||||
# Bolt EV Premier no ACC 2021
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 257: 8, 288: 5, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 390: 7, 407: 7, 417: 7, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 513: 6, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 7, 567: 5, 568: 1, 569: 3, 573: 1, 577: 8, 578: 8, 579: 8, 592: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 717: 5, 753: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 975: 2, 977: 8, 979: 7, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1037: 5, 1105: 5, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1236: 8, 1243: 3, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1279: 4, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1346: 8, 1347: 8, 1513: 8, 1516: 8, 1601: 8, 1616: 8, 1904: 7, 1905: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1922: 7, 1927: 7, 2016: 8, 2017: 8, 2018: 8, 2020: 8, 2023: 8, 2024: 8, 2028: 8, 2031: 8
|
||||
},
|
||||
# shermy99's Bolt EV Premier no ACC 2023
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 3, 241: 6, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 308: 4, 309: 8, 311: 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, 390: 7, 398: 8, 407: 7, 417: 8, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 513: 6, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 8, 567: 5, 568: 2, 569: 3, 573: 1, 577: 8, 579: 8, 592: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 711: 6, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 866: 4, 869: 4, 872: 1, 880: 6, 961: 8, 967: 4, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1010: 8, 1013: 6, 1015: 1, 1017: 8, 1019: 2, 1020: 8, 1037: 5, 1105: 5, 1187: 5, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1347: 8, 1513: 8, 1516: 8, 1601: 8, 1609: 8, 1613: 8, 1616: 8, 1618: 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, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1913: 7, 1920: 8, 1922: 7, 1924: 8, 1927: 7, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1984: 8, 1988: 8, 2000: 8, 2001: 8, 2002: 8, 2017: 8, 2018: 8, 2020: 8, 2023: 8, 2025: 8, 2028: 8, 2031: 8
|
||||
},
|
||||
{
|
||||
170: 8, 188: 8, 189: 7, 190: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 3, 241: 6, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 353: 3, 381: 8, 384: 4, 386: 8, 388: 8, 390: 7, 398: 8, 407: 7, 417: 8, 419: 1, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 513: 6, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 564: 5, 566: 8, 567: 5, 568: 2, 569: 3, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 711: 6, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 866: 4, 869: 4, 872: 1, 880: 6, 961: 8, 967: 4, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 988: 6, 989: 8, 995: 7, 1001: 8, 1009: 8, 1010: 8, 1013: 6, 1015: 1, 1017: 8, 1019: 2, 1020: 8, 1105: 5, 1187: 5, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1265: 8, 1275: 3, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1913: 7, 1927: 7, 1930: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8
|
||||
}],
|
||||
CAR.CHEVROLET_EQUINOX_NON_ACC_3RD_GEN: [{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 401: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 444: 7, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 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, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1273: 3, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1611: 8, 1618: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1920: 7, 1930: 7
|
||||
},
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 384: 4, 393: 8, 398: 8, 401: 8, 413: 8, 422: 4, 431: 8, 444: 7, 453: 6, 456: 8, 479: 3, 481: 7, 485: 8, 499: 3, 500: 6, 501: 8, 567: 5, 647: 6, 800: 6, 1033: 7, 1034: 7, 1296: 4
|
||||
}],
|
||||
# Trailblazer also matches as a Silverado, so comment out to avoid conflicts.
|
||||
# TODO-SP: split with FW versions
|
||||
# CAR.TRAILBLAZER: [
|
||||
# {
|
||||
# 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 587: 8, 707: 8, 715: 8, 717: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1609: 8, 1611: 8, 1613: 8, 1649: 8, 1792: 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, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8
|
||||
# }],
|
||||
CAR.CHEVROLET_SUBURBAN_NON_ACC_11TH_GEN: [
|
||||
# Slav's 2018 Suburban, LKAS no ACC
|
||||
{
|
||||
170: 8, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 493: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 562: 8, 563: 5, 564: 5, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 717: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1920: 7
|
||||
},
|
||||
# Qube's 2017 Suburban, LKAS no ACC
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 460: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 497: 8, 500: 6, 501: 8, 510: 8, 528: 5, 532: 6, 534: 2, 562: 8, 563: 5, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 717: 5, 761: 7, 800: 6, 801: 8, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1609: 8, 1611: 8, 1613: 8, 1649: 8, 1792: 8, 1793: 8, 1798: 8, 1799: 8, 1810: 8, 1813: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1856: 8, 1858: 8, 1859: 8, 1860: 8, 1862: 8, 1863: 8, 1871: 8, 1872: 8, 1875: 8, 1879: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1920: 8, 1924: 8, 1927: 8, 1937: 8, 1953: 8, 1954: 8, 1955: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1984: 8, 1988: 8, 1990: 8, 2000: 8, 2001: 8, 2004: 8, 2017: 8, 2018: 8, 2020: 8
|
||||
}],
|
||||
CAR.CADILLAC_CT6_NON_ACC_1ST_GEN: [
|
||||
# badgers4life's 2017 CT6
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 4, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 393: 7, 398: 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, 456: 8, 460: 5, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 3, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 717: 5, 723: 2, 753: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 961: 8, 969: 8, 977: 8, 979: 7, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 1, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 7, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1793: 8, 1798: 8, 1799: 8, 1810: 8, 1813: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1856: 8, 1858: 8, 1859: 8, 1860: 8, 1862: 8, 1863: 8, 1872: 8, 1875: 8, 1879: 8, 1882: 8, 1888: 4, 1889: 8, 1892: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1919: 7, 1920: 8, 1924: 8, 1927: 8, 1928: 7, 1937: 8, 1953: 8, 1954: 8, 1955: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1984: 8, 1988: 8, 2000: 8, 2001: 8, 2002: 8, 2004: 8, 2017: 8, 2018: 8, 2020: 8, 2026: 8
|
||||
}],
|
||||
CAR.CHEVROLET_TRAILBLAZER_NON_ACC_2ND_GEN: [
|
||||
{
|
||||
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.CHEVROLET_MALIBU_NON_ACC_9TH_GEN: [
|
||||
# Verylukyguy's Malibu
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 328: 1, 352: 5, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 409: 8, 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, 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, 717: 5, 730: 4, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 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: 6, 1189: 1, 1195: 3, 1217: 8, 1221: 5, 1223: 3, 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, 1279: 4, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7
|
||||
},
|
||||
# Tesla's Malibu
|
||||
{
|
||||
189: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 6, 390: 8, 392: 4, 393: 4, 394: 8, 395: 3, 396: 4, 397: 4, 398: 4, 399: 4, 400: 4, 401: 4, 402: 4, 403: 4, 404: 4, 405: 4, 406: 4, 407: 4, 408: 4, 409: 4, 410: 4, 411: 4, 412: 4, 413: 4, 414: 4, 415: 4, 416: 4, 417: 4, 418: 4, 419: 4, 420: 4, 421: 4, 422: 4, 423: 4, 424: 4, 425: 4, 426: 4, 427: 4, 428: 4, 429: 4, 430: 4, 431: 4, 432: 4, 433: 4, 434: 4, 435: 4, 436: 4, 437: 4, 438: 4, 439: 4, 440: 4, 441: 4, 442: 4, 443: 4, 444: 4, 445: 4, 446: 4, 447: 4, 448: 4, 449: 4, 450: 4, 451: 8, 452: 8, 453: 6, 479: 4, 481: 7, 485: 8, 489: 8, 493: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 566: 6, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 717: 5, 761: 7, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1922: 7
|
||||
}],
|
||||
CAR.CADILLAC_XT5_NON_ACC_1ST_GEN: [
|
||||
# TRain's 2017 XT5
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 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, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 510: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 647: 3, 707: 8, 717: 5, 723: 2, 753: 5, 761: 7, 800: 6, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 872: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1914: 7, 1919: 7, 1920: 7
|
||||
}],
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import os
|
||||
from math import exp
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.common.basedir import BASEDIR
|
||||
from opendbc.car.gm.interface import CAR
|
||||
from opendbc.sunnypilot.car.interfaces import LatControlInputs, NanoFFModel, TorqueFromLateralAccelCallbackTypeTorqueSpace
|
||||
|
||||
NEURAL_PARAMS_PATH = os.path.join(BASEDIR, '../sunnypilot/car/torque_data/neural_ff_weights.json')
|
||||
|
||||
NON_LINEAR_TORQUE_PARAMS = {
|
||||
CAR.CHEVROLET_BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
|
||||
CAR.GMC_ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772],
|
||||
CAR.CHEVROLET_SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122]
|
||||
}
|
||||
|
||||
|
||||
class CarInterfaceExt:
|
||||
def __init__(self, CP: structs.CarParams, CI_Base):
|
||||
self.CP = CP
|
||||
self.CI_Base = CI_Base
|
||||
self.neural_ff_model = None
|
||||
|
||||
def torque_from_lateral_accel_siglin(self, latcontrol_inputs: LatControlInputs, torque_params: structs.CarParams.LateralTorqueTuning,
|
||||
gravity_adjusted: bool) -> float:
|
||||
def sig(val):
|
||||
# https://timvieira.github.io/blog/post/2014/02/11/exp-normalize-trick
|
||||
if val >= 0:
|
||||
return 1 / (1 + exp(-val)) - 0.5
|
||||
else:
|
||||
z = exp(val)
|
||||
return z / (1 + z) - 0.5
|
||||
|
||||
# The "lat_accel vs torque" relationship is assumed to be the sum of "sigmoid + linear" curves
|
||||
# An important thing to consider is that the slope at 0 should be > 0 (ideally >1)
|
||||
# This has big effect on the stability about 0 (noise when going straight)
|
||||
# ToDo: To generalize to other GMs, explore tanh function as the nonlinear
|
||||
non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint)
|
||||
assert non_linear_torque_params, "The params are not defined"
|
||||
a, b, c, _ = non_linear_torque_params
|
||||
steer_torque = (sig(latcontrol_inputs.lateral_acceleration * a) * b) + (latcontrol_inputs.lateral_acceleration * c)
|
||||
return float(steer_torque)
|
||||
|
||||
def torque_from_lateral_accel_neural(self, latcontrol_inputs: LatControlInputs, orque_params: structs.CarParams.LateralTorqueTuning,
|
||||
gravity_adjusted: bool) -> float:
|
||||
inputs = list(latcontrol_inputs)
|
||||
if gravity_adjusted:
|
||||
inputs[0] += inputs[1]
|
||||
return float(self.neural_ff_model.predict(inputs))
|
||||
|
||||
def torque_from_lateral_accel_in_torque_space(self) -> TorqueFromLateralAccelCallbackTypeTorqueSpace:
|
||||
if self.CP.carFingerprint == CAR.CHEVROLET_BOLT_EUV:
|
||||
self.neural_ff_model = NanoFFModel(NEURAL_PARAMS_PATH, self.CP.carFingerprint)
|
||||
return self.torque_from_lateral_accel_neural
|
||||
elif self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS:
|
||||
return self.torque_from_lateral_accel_siglin
|
||||
else:
|
||||
return self.CI_Base.torque_from_lateral_accel_linear_in_torque_space
|
||||
@@ -0,0 +1,16 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from enum import IntFlag
|
||||
|
||||
|
||||
class GMFlagsSP(IntFlag):
|
||||
NON_ACC = 1
|
||||
|
||||
|
||||
class GMSafetyFlagsSP:
|
||||
NON_ACC = 1
|
||||
Whitespace-only changes.
@@ -0,0 +1,33 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from enum import StrEnum
|
||||
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.sunnypilot.car.honda.values_ext import HondaFlagsSP
|
||||
|
||||
|
||||
class CarStateExt:
|
||||
def __init__(self, CP, CP_SP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
def update(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
|
||||
if self.CP_SP.flags & HondaFlagsSP.NIDEC_HYBRID:
|
||||
ret.accFaulted = bool(cp.vl["HYBRID_BRAKE_ERROR"]["BRAKE_ERROR_1"] or cp.vl["HYBRID_BRAKE_ERROR"]["BRAKE_ERROR_2"])
|
||||
ret.stockAeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE_HYBRID"] > 1e-5)
|
||||
|
||||
if self.CP_SP.flags & HondaFlagsSP.HYBRID_ALT_BRAKEHOLD:
|
||||
ret.brakeHoldActive = cp.vl["BRAKE_HOLD_HYBRID_ALT"]["BRAKE_HOLD_ACTIVE"] == 1
|
||||
|
||||
if self.CP_SP.enableGasInterceptor:
|
||||
# Same threshold as panda, equivalent to 1e-5 with previous DBC scaling
|
||||
gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
|
||||
ret.gasPressed = gas > 492
|
||||
@@ -0,0 +1,59 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.honda.values import CAR
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
FW_VERSIONS_EXT = {
|
||||
CAR.HONDA_ACCORD: {
|
||||
(Ecu.eps, 0x18da30f1, None): [
|
||||
b'39990-TVA,A150\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.HONDA_CIVIC: {
|
||||
(Ecu.eps, 0x18da30f1, None): [
|
||||
b'39990-TBA,A030\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.HONDA_CIVIC_BOSCH: {
|
||||
(Ecu.eps, 0x18da30f1, None): [
|
||||
b'39990-TGG,A020\x00\x00',
|
||||
b'39990-TGG,A120\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.HONDA_CRV_5G: {
|
||||
(Ecu.eps, 0x18da30f1, None): [
|
||||
b'39990-TLA,A040\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.HONDA_CLARITY: {
|
||||
(Ecu.shiftByWire, 0x18da0bf1, None): [
|
||||
b'54008-TRW-A910\x00\x00',
|
||||
],
|
||||
(Ecu.vsa, 0x18da28f1, None): [
|
||||
b'57114-TRW-A010\x00\x00',
|
||||
b'57114-TRW-A020\x00\x00',
|
||||
],
|
||||
(Ecu.eps, 0x18da30f1, None): [
|
||||
b'39990-TRW-A020\x00\x00',
|
||||
b'39990-TRW,A020\x00\x00', # modified firmware
|
||||
b'39990,TRW,A020\x00\x00', # extra modified firmware
|
||||
],
|
||||
(Ecu.srs, 0x18da53f1, None): [
|
||||
b'77959-TRW-A210\x00\x00',
|
||||
b'77959-TRW-A220\x00\x00',
|
||||
],
|
||||
(Ecu.gateway, 0x18daeff1, None): [
|
||||
b'38897-TRW-A010\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x18dab0f1, None): [
|
||||
b'36161-TRW-A110\x00\x00',
|
||||
],
|
||||
},
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.can_definitions import CanData
|
||||
from opendbc.sunnypilot.car import create_gas_interceptor_command
|
||||
|
||||
|
||||
class GasInterceptorCarController:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
self.gas = 0.
|
||||
self.interceptor_gas_cmd = 0.
|
||||
|
||||
def update(self, CC: structs.CarControl, CS: structs.CarState, gas: float, brake: float, wind_brake: float,
|
||||
packer, frame: int) -> list[CanData]:
|
||||
can_sends = []
|
||||
|
||||
if self.CP_SP.enableGasInterceptor:
|
||||
# way too aggressive at low speed without this
|
||||
gas_mult = np.interp(CS.out.vEgo, [0., 10.], [0.4, 1.0])
|
||||
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
|
||||
# This prevents unexpected pedal range rescaling
|
||||
# Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
|
||||
# when you do enable.
|
||||
if CC.longActive:
|
||||
self.gas = float(np.clip(gas_mult * (gas - brake + wind_brake * 3 / 4), 0., 1.))
|
||||
else:
|
||||
self.gas = 0.0
|
||||
can_sends.append(create_gas_interceptor_command(packer, self.gas, frame // 2))
|
||||
|
||||
return can_sends
|
||||
@@ -0,0 +1,41 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from opendbc.car import structs, DT_CTRL
|
||||
from opendbc.car.can_definitions import CanData
|
||||
from opendbc.car.honda import hondacan
|
||||
from opendbc.car.honda.values import CruiseButtons
|
||||
from opendbc.sunnypilot.car.intelligent_cruise_button_management_interface_base import IntelligentCruiseButtonManagementInterfaceBase
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
SendButtonState = structs.IntelligentCruiseButtonManagement.SendButtonState
|
||||
|
||||
BUTTONS = {
|
||||
SendButtonState.increase: CruiseButtons.RES_ACCEL,
|
||||
SendButtonState.decrease: CruiseButtons.DECEL_SET,
|
||||
}
|
||||
|
||||
|
||||
class IntelligentCruiseButtonManagementInterface(IntelligentCruiseButtonManagementInterfaceBase):
|
||||
def __init__(self, CP, CP_SP):
|
||||
super().__init__(CP, CP_SP)
|
||||
|
||||
def update(self, CC_SP, packer, frame, last_button_frame, CAN) -> list[CanData]:
|
||||
can_sends = []
|
||||
self.CC_SP = CC_SP
|
||||
self.ICBM = CC_SP.intelligentCruiseButtonManagement
|
||||
self.frame = frame
|
||||
self.last_button_frame = last_button_frame
|
||||
|
||||
if self.ICBM.sendButton != SendButtonState.none:
|
||||
send_button = BUTTONS[self.ICBM.sendButton]
|
||||
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL > 0.05:
|
||||
can_sends.append(hondacan.spam_buttons_command(packer, CAN, send_button, self.CP.carFingerprint))
|
||||
self.last_button_frame = self.frame
|
||||
|
||||
return can_sends
|
||||
@@ -0,0 +1,22 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.honda.values import HONDA_BOSCH_RADARLESS
|
||||
|
||||
|
||||
class MadsCarController:
|
||||
def __init__(self):
|
||||
self.dashed_lanes = False
|
||||
|
||||
def update(self, CP: structs.CarParams, CC: structs.CarControl, CC_SP: structs.CarControlSP) -> None:
|
||||
enable_mads = CC_SP.mads.available
|
||||
|
||||
if enable_mads:
|
||||
self.dashed_lanes = CC_SP.mads.enabled and not CC.latActive
|
||||
else:
|
||||
self.dashed_lanes = CC.hudControl.lanesVisible if CP.carFingerprint in HONDA_BOSCH_RADARLESS else False
|
||||
@@ -0,0 +1,31 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import unittest
|
||||
|
||||
from opendbc.testing import parameterized
|
||||
|
||||
from opendbc.car import gen_empty_fingerprint
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.honda.values import CAR
|
||||
|
||||
CarFw = CarParams.CarFw
|
||||
|
||||
|
||||
class TestHondaEpsMod(unittest.TestCase):
|
||||
|
||||
@parameterized("car_name, fw", [(CAR.HONDA_CIVIC, b'39990-TBA,A030\x00\x00'), (CAR.HONDA_CIVIC, b'39990-TBA-A030\x00\x00'),
|
||||
(CAR.HONDA_CLARITY, b'39990-TRW-A020\x00\x00'), (CAR.HONDA_CLARITY, b'39990,TRW,A020\x00\x00')])
|
||||
def test_eps_mod_fingerprint(self, car_name, fw):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
car_fw = [CarFw(ecu="eps", fwVersion=fw)]
|
||||
|
||||
CarInterface = interfaces[car_name]
|
||||
CP = CarInterface.get_params(car_name, fingerprint, car_fw, False, False, False)
|
||||
_ = CarInterface.get_params_sp(CP, car_name, fingerprint, car_fw, False, False, False)
|
||||
|
||||
self.assertFalse(CP.dashcamOnly)
|
||||
@@ -0,0 +1,19 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from enum import IntFlag
|
||||
|
||||
|
||||
class HondaFlagsSP(IntFlag):
|
||||
NIDEC_HYBRID = 1
|
||||
EPS_MODIFIED = 2
|
||||
HYBRID_ALT_BRAKEHOLD = 4
|
||||
|
||||
|
||||
class HondaSafetyFlagsSP:
|
||||
NIDEC_HYBRID = 1
|
||||
GAS_INTERCEPTOR = 2
|
||||
Whitespace-only changes.
@@ -0,0 +1,86 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from enum import StrEnum
|
||||
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.car.hyundai.values import HyundaiFlags
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
|
||||
|
||||
class CarStateExt:
|
||||
def __init__(self, CP, CP_SP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
self.aBasis = 0.0
|
||||
|
||||
def update_speed_limit(self, cp, cp_cam) -> float:
|
||||
speed_limit = 0
|
||||
|
||||
if self.CP.flags & HyundaiFlags.CANFD:
|
||||
if self.CP_SP.flags & HyundaiFlagsSP.SPEED_LIMIT_AVAILABLE:
|
||||
bus = cp if self.CP.flags & HyundaiFlags.CANFD_LKA_STEER_MSG else cp_cam
|
||||
speed_limit = bus.vl["FR_CMR_02_100ms"]["ISLW_SpdCluMainDis"]
|
||||
else:
|
||||
nav, cam = 0, 0
|
||||
if self.CP_SP.flags & HyundaiFlagsSP.SPEED_LIMIT_AVAILABLE:
|
||||
nav = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"]
|
||||
if self.CP_SP.flags & HyundaiFlagsSP.HAS_LKAS12:
|
||||
cam = cp_cam.vl["LKAS12"]["CF_Lkas_TsrSpeed_Display_Clu"]
|
||||
|
||||
speed_limit = cam if cam not in (0, 255) else nav
|
||||
|
||||
if speed_limit in (0, 255):
|
||||
speed_limit = 0
|
||||
|
||||
return speed_limit
|
||||
|
||||
def update(self, ret: structs.CarState, ret_sp: structs.CarStateSP, can_parsers: dict[StrEnum, CANParser], speed_conv: float) -> None:
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
|
||||
self.aBasis = cp.vl["TCS13"]["aBasis"]
|
||||
|
||||
if self.CP_SP.flags & HyundaiFlagsSP.NON_SCC:
|
||||
cruise_msg = "LABEL11" if self.CP.flags & HyundaiFlags.EV else \
|
||||
"E_CRUISE_CONTROL" if self.CP.flags & HyundaiFlags.HYBRID else \
|
||||
"EMS16"
|
||||
cruise_available_sig = "CC_React" if self.CP.flags & HyundaiFlags.EV else "CRUISE_LAMP_M"
|
||||
cruise_enabled_sig = "CC_ACT" if self.CP.flags & HyundaiFlags.EV else "CRUISE_LAMP_S"
|
||||
cruise_speed_msg = "E_EMS11" if self.CP.flags & HyundaiFlags.EV else \
|
||||
"ELECT_GEAR" if self.CP.flags & HyundaiFlags.HYBRID else \
|
||||
"LVR12"
|
||||
cruise_speed_sig = "Cruise_Limit_Target" if self.CP.flags & HyundaiFlags.EV else \
|
||||
"SLC_SET_SPEED" if self.CP.flags & HyundaiFlags.HYBRID else \
|
||||
"CF_Lvr_CruiseSet"
|
||||
ret.cruiseState.available = cp.vl[cruise_msg][cruise_available_sig] != 0
|
||||
ret.cruiseState.enabled = cp.vl[cruise_msg][cruise_enabled_sig] != 0
|
||||
ret.cruiseState.speed = cp.vl[cruise_speed_msg][cruise_speed_sig] * speed_conv
|
||||
ret.cruiseState.standstill = False
|
||||
ret.cruiseState.nonAdaptive = False
|
||||
|
||||
if not self.CP_SP.flags & HyundaiFlagsSP.NON_SCC_NO_FCA:
|
||||
cp_cruise = cp if self.CP_SP.flags & HyundaiFlagsSP.NON_SCC_RADAR_FCA else cp_cam
|
||||
|
||||
aeb_src = "FCA11"
|
||||
aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0
|
||||
aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src]["FCA_CmdAct"] != 0
|
||||
ret.stockFcw = aeb_warning and not aeb_braking
|
||||
ret.stockAeb = aeb_warning and aeb_braking
|
||||
|
||||
ret_sp.speedLimit = self.update_speed_limit(cp, cp_cam) * speed_conv
|
||||
|
||||
def update_canfd_ext(self, ret: structs.CarState, ret_sp: structs.CarStateSP, can_parsers: dict[StrEnum, CANParser],
|
||||
speed_factor: float) -> None:
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
|
||||
self.aBasis = cp.vl["TCS"]["aBasis"]
|
||||
|
||||
ret_sp.speedLimit = self.update_speed_limit(cp, cp_cam) * speed_factor
|
||||
@@ -0,0 +1,74 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from opendbc.car import uds
|
||||
from opendbc.car.carlog import carlog
|
||||
from opendbc.car.isotp_parallel_query import IsoTpParallelQuery
|
||||
|
||||
DEVELOPER_DIAGNOSTIC = 0x07
|
||||
CUSTOM_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, DEVELOPER_DIAGNOSTIC])
|
||||
CUSTOM_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, DEVELOPER_DIAGNOSTIC])
|
||||
|
||||
READ_DATA_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER])
|
||||
READ_DATA_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40])
|
||||
|
||||
WRITE_DATA_REQUEST = bytes([uds.SERVICE_TYPE.WRITE_DATA_BY_IDENTIFIER])
|
||||
WRITE_DATA_RESPONSE = bytes([uds.SERVICE_TYPE.WRITE_DATA_BY_IDENTIFIER + 0x40])
|
||||
|
||||
CONFIG_DATA_ID = bytes([0x01, 0x42])
|
||||
DEFAULT_CONFIG = bytes([0x00, 0x00, 0x00, 0x01, 0x00, 0x00])
|
||||
TRACKS_ENABLED_CONFIG = bytes([0x00, 0x00, 0x00, 0x01, 0x00, 0x01])
|
||||
TRACKS_ENABLED_CONFIG_BYTES = b"\x00\x00\x01\x00\x01"
|
||||
|
||||
|
||||
def enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, timeout=0.1, retry=2):
|
||||
carlog.error("radar_tracks: enabling ...")
|
||||
|
||||
for i in range(retry):
|
||||
try:
|
||||
query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [CUSTOM_DIAGNOSTIC_REQUEST], [CUSTOM_DIAGNOSTIC_RESPONSE])
|
||||
|
||||
for _, _ in query.get_data(timeout).items():
|
||||
carlog.error("radar_tracks: check current config ...")
|
||||
|
||||
request = READ_DATA_REQUEST + CONFIG_DATA_ID
|
||||
query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [request], [READ_DATA_RESPONSE])
|
||||
|
||||
for _, data in query.get_data(timeout).items():
|
||||
current_config = data[3:]
|
||||
|
||||
carlog.error(f"radar_tracks: current config: {current_config.hex()}")
|
||||
|
||||
if current_config == TRACKS_ENABLED_CONFIG_BYTES:
|
||||
carlog.error("radar_tracks: already enabled, skipping ...")
|
||||
else:
|
||||
carlog.error("radar_tracks: reconfigure radar to output radar points ...")
|
||||
request = WRITE_DATA_REQUEST + CONFIG_DATA_ID + TRACKS_ENABLED_CONFIG
|
||||
query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [request], [WRITE_DATA_RESPONSE])
|
||||
query.get_data(0)
|
||||
|
||||
carlog.error("radar_tracks: successfully enabled")
|
||||
|
||||
return True
|
||||
|
||||
except Exception as e:
|
||||
carlog.exception(f"radar_tracks exception: {e}")
|
||||
|
||||
carlog.error(f"radar_tracks retry ({i + 1}) ...")
|
||||
carlog.error("radar_tracks: failed")
|
||||
return False
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import time
|
||||
import openpilot.cereal.messaging as messaging
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
logcan = messaging.sub_sock('can')
|
||||
time.sleep(1)
|
||||
|
||||
enabled = enable_radar_tracks(logcan, sendcan, bus=0, addr=0x7d0, timeout=0.1)
|
||||
print(f"enabled: {enabled}")
|
||||
@@ -0,0 +1,68 @@
|
||||
from opendbc.car import structs
|
||||
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
|
||||
ESCC_MSG = 0x2AB
|
||||
|
||||
|
||||
class EnhancedSmartCruiseControl:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
@property
|
||||
def enabled(self):
|
||||
return self.CP_SP.flags & HyundaiFlagsSP.ENHANCED_SCC
|
||||
|
||||
@property
|
||||
def trigger_msg(self):
|
||||
return ESCC_MSG
|
||||
|
||||
def update_car_state(self, car_state):
|
||||
"""
|
||||
This method is invoked by the CarController to update the car state on the ESCC object.
|
||||
The updated state is then used to update SCC12 with the current car state values received through ESCC.
|
||||
:param car_state:
|
||||
:return:
|
||||
"""
|
||||
self.car_state = car_state
|
||||
|
||||
def update_scc12(self, values):
|
||||
"""
|
||||
Update SCC12 with the current car state values received through ESCC.
|
||||
These values are sourced directly from the car's SCC radar and provide a more reliable source for AEB and FCA alerts.
|
||||
:param values: SCC12 to be sent in dictionary form before being packed
|
||||
:return: Nothing. SCC12 is updated in place.
|
||||
"""
|
||||
values["AEB_CmdAct"] = self.car_state.escc_cmd_act
|
||||
values["CF_VSM_Warn"] = self.car_state.escc_aeb_warning
|
||||
values["CF_VSM_DecCmdAct"] = self.car_state.escc_aeb_dec_cmd_act
|
||||
values["CR_VSM_DecCmd"] = self.car_state.escc_aeb_dec_cmd
|
||||
# TODO-SP: we should read it from the car's settings and use that value.
|
||||
# It may not be ideal to set this here directly.
|
||||
# Observed flickering on the dashboard settings switching between "deactivated" and "active assistance" when sending AEB_Status = 1.
|
||||
# These values could differ from the user's configuration from the car's settings.
|
||||
# This indicates that SCC12 likely displays it on the dashboard, and another FCA message may also cause it to appear.
|
||||
values["AEB_Status"] = 2 # AEB enabled
|
||||
|
||||
|
||||
class EsccCarStateBase:
|
||||
def __init__(self):
|
||||
self.escc_aeb_warning = 0
|
||||
self.escc_aeb_dec_cmd_act = 0
|
||||
self.escc_cmd_act = 0
|
||||
self.escc_aeb_dec_cmd = 0
|
||||
|
||||
|
||||
class EsccCarController:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
self.ESCC = EnhancedSmartCruiseControl(CP, CP_SP)
|
||||
|
||||
def update(self, car_state):
|
||||
self.ESCC.update_car_state(car_state)
|
||||
|
||||
|
||||
class EsccRadarInterfaceBase:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
self.ESCC = EnhancedSmartCruiseControl(CP, CP_SP)
|
||||
self.use_escc = False
|
||||
@@ -0,0 +1,126 @@
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.hyundai.values import CAR
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
|
||||
FW_VERSIONS_EXT = {
|
||||
CAR.KIA_CEED_PHEV_2022_NON_SCC: {
|
||||
(Ecu.eps, 0x7D4, None): [
|
||||
b'\xf1\x00CD MDPS C 1.00 1.01 56310-XX000 4CPHC101',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x7C4, None): [
|
||||
b'\xf1\x00CDH LKAS AT EUR LHD 1.00 1.01 99211-CR700 931',
|
||||
],
|
||||
},
|
||||
# TODO-SP: HYUNDAI_KONA_EV_NON_SCC has the same FW versions as HYUNDAI_KONA_EV, in the future we may
|
||||
# allow similar FW versions across different platforms
|
||||
# CAR.HYUNDAI_KONA_EV_NON_SCC: {
|
||||
# (Ecu.abs, 0x7d1, None): [
|
||||
# b'\xf1\x00OS IEB \x02 212 \x11\x13 58520-K4000',
|
||||
# ],
|
||||
# (Ecu.eps, 0x7d4, None): [
|
||||
# b'\xf1\x00OS MDPS C 1.00 1.04 56310K4000\x00 4OEDC104',
|
||||
# ],
|
||||
# (Ecu.fwdCamera, 0x7c4, None): [
|
||||
# b'\xf1\x00OSE LKAS AT USA LHD 1.00 1.00 95740-K4100 W40',
|
||||
# ],
|
||||
# },
|
||||
CAR.GENESIS_G70_2021_NON_SCC: {
|
||||
(Ecu.eps, 0x7d4, None): [
|
||||
b'\xf1\x00IK MDPS R 1.00 1.08 57700-G9200 4I2CL108',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x7d0, None): [
|
||||
b'\xf1\x00IK__ SCC --CUP 1.00 1.02 96400-G9100 ',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00IK MFC MT USA LHD 1.00 1.01 95740-G9000 170920',
|
||||
],
|
||||
},
|
||||
CAR.HYUNDAI_KONA_NON_SCC: {
|
||||
# (Ecu.abs, 0x7d1, None): [
|
||||
# b'\xf1\x816V5RAJ00040.ELF\xf1\x00\x00\x00\x00\x00\x00\x00',
|
||||
# ],
|
||||
(Ecu.eps, 0x7d4, None): [
|
||||
b'\xf1\x00OS MDPS C 1.00 1.05 56310J9030\x00 4OSDC105',
|
||||
b'\xf1\x00OS MDPS C 1.00 1.04 56310J9030\x00 4OSDC104',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00OS9 LKAS AT USA LHD 1.00 1.00 95740-J9200 g30',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xf1\x006T6J0_C2\x00\x006T6K1051\x00\x00TOS4N20NS2\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.KIA_FORTE_2019_NON_SCC: {
|
||||
(Ecu.eps, 0x7D4, None): [
|
||||
b'\xf1\x00BD MDPS C 1.00 1.04 56310/M6000 4BDDC104',
|
||||
b'\xf1\x00BD MDPS C 1.00 1.05 56310/M6000 4BDDC105',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x7C4, None): [
|
||||
b'\xf1\x00BD LKAS AT USA LHD 1.00 1.02 95740-M6000 J31',
|
||||
],
|
||||
# (Ecu.abs, 0x7d1, None): [
|
||||
# b'\xf1\x816VFRAF00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00',
|
||||
# ],
|
||||
# (Ecu.transmission, 0x7e1, None): [
|
||||
# b'\xf1\x87CXJQAM4966515JB0x\xa9\x98\x9b\x99fff\x98feg\x88\x88w\x88Ff\x8f\xff{\xff\xff\xff\xa8\xf6\xf1\x816V2C1051\x00\x00\xf1\x006V2B0_C2\x00\x006V2C1051\x00\x00CBD0N20NS8q\xc1&\xd2', # noqa: E501
|
||||
# ],
|
||||
},
|
||||
CAR.KIA_FORTE_2021_NON_SCC: {
|
||||
(Ecu.eps, 0x7D4, None): [
|
||||
b'\xf1\x00BD MDPS C 1.00 1.08 56310M6000\x00 4BDDC108',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x7C4, None): [
|
||||
b'\xf1\x00BD LKAS AT USA LHD 1.00 1.04 95740-M6000 J33',
|
||||
],
|
||||
# (Ecu.abs, 0x7d1, None): [
|
||||
# b'\xf1\x816VFRAL00010.ELF\xf1\x00\x00\x00\x00\x00\x00\x00',
|
||||
# ],
|
||||
# (Ecu.transmission, 0x7e1, None): [
|
||||
# b'\xf1\x87CXLQAM0906975JB0\x89\x88\xa6\x8aVfug\xba\x87\x94yffuxgfo\xff\x8b\xff\xff\xff\x91\x82\xf1\x816V2C1051\x00\x00\xf1\x006V2B0_C2\x00\x006V2C1051\x00\x00CBD0N20NS8q\xc1&\xd2', # noqa: E501
|
||||
# ],
|
||||
},
|
||||
CAR.KIA_SELTOS_2023_NON_SCC: {
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x00SP ESC \t 101"\t\x01 58910-Q5510',
|
||||
b'\xf1\x00SP ESC \r 100"\x04\x01 58910-Q5510',
|
||||
],
|
||||
(Ecu.eps, 0x7d4, None): [
|
||||
b'\xf1\x00SP2 MDPS C 1.00 1.04 56310Q5240 4SPSC104',
|
||||
b'\xf1\x00SP2 MDPS C 1.00 1.01 56300Q5920 ',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00SP2 MFC AT USA LHD 1.00 1.03 99210-Q5500 230208',
|
||||
b'\xf1\x00SP2 MFC AT AUS RHD 1.00 1.02 99210-Q5500 220624',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xf1\x006V2B0_C2\x00\x006V2D5051\x00\x00CSP2N20NL0\x00\x00\x00\x00',
|
||||
b'\xf1\x006V2B0_C2\x00\x006V2D4051\x00\x00CSP2N20KL1\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.HYUNDAI_ELANTRA_2022_NON_SCC: {
|
||||
(Ecu.eps, 0x7d4, None): [
|
||||
# b'\xf1\x8756310AA030\x00\xf1\x00CN7 MDPS C 1.00 1.06 56310AA030\x00 4CNDC106',
|
||||
b'\xf1\x00CN7 MDPS R 1.00 1.04 57700-IB000 4CNNP104',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.01 99210-AB000 210205',
|
||||
b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.00 99210-IB000 210531',
|
||||
],
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
# b'\xf1\x8758910-AB500\xf1\x00CN ESC \t 100 \x06\x01 58910-AB500',
|
||||
b'\xf1\x00CN ESC \t 100!\x05\x01 58910-IB000',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
# b'\xf1\x87CXNQEM4091445JB3g\x98\x98\x89\x99\x87gv\x89wuwgwv\x89hD_\xffx\xff\xff\xff\x86\xeb\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00', # noqa: E501
|
||||
b'\xf1\x00T02601BL T02900A1 WCN7T20XXX900NS4\xf7\xccz\xf6',
|
||||
],
|
||||
},
|
||||
CAR.HYUNDAI_BAYON_1ST_GEN_NON_SCC: {
|
||||
# TODO: Check working route for more FW
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00BC3 LKA AT EUR LHD 1.00 1.01 99211-Q0100 261'
|
||||
],
|
||||
},
|
||||
}
|
||||
@@ -0,0 +1,78 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
from opendbc.car import DT_CTRL, structs
|
||||
from opendbc.car.can_definitions import CanData
|
||||
from opendbc.car.hyundai import hyundaican, hyundaicanfd
|
||||
from opendbc.car.hyundai.values import HyundaiFlags, Buttons, CANFD_CAR
|
||||
from opendbc.sunnypilot.car.intelligent_cruise_button_management_interface_base import IntelligentCruiseButtonManagementInterfaceBase
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
SendButtonState = structs.IntelligentCruiseButtonManagement.SendButtonState
|
||||
|
||||
BUTTON_COPIES = 2
|
||||
BUTTON_COPIES_TIME = 7
|
||||
BUTTON_COPIES_TIME_IMPERIAL = [BUTTON_COPIES_TIME + 3, 70]
|
||||
BUTTON_COPIES_TIME_METRIC = [BUTTON_COPIES_TIME, 40]
|
||||
|
||||
BUTTONS = {
|
||||
SendButtonState.increase: Buttons.RES_ACCEL,
|
||||
SendButtonState.decrease: Buttons.SET_DECEL,
|
||||
}
|
||||
|
||||
|
||||
class IntelligentCruiseButtonManagementInterface(IntelligentCruiseButtonManagementInterfaceBase):
|
||||
def __init__(self, CP, CP_SP):
|
||||
super().__init__(CP, CP_SP)
|
||||
|
||||
def create_can_mock_button_messages(self, packer, CS, send_button) -> list[CanData]:
|
||||
can_sends = []
|
||||
copies_xp = BUTTON_COPIES_TIME_METRIC if CS.is_metric else BUTTON_COPIES_TIME_IMPERIAL
|
||||
copies = int(np.interp(BUTTON_COPIES_TIME, copies_xp, [1, BUTTON_COPIES]))
|
||||
|
||||
# send resume at a max freq of 10Hz
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
|
||||
# send 25 messages at a time to increases the likelihood of resume being accepted
|
||||
can_sends.extend([hyundaican.create_clu11(packer, self.frame, CS.clu11, send_button, self.CP)] * copies)
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15:
|
||||
self.last_button_frame = self.frame
|
||||
|
||||
return can_sends
|
||||
|
||||
def create_canfd_mock_button_messages(self, packer, CS, CAN, send_button) -> list[CanData]:
|
||||
can_sends = []
|
||||
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
|
||||
# TODO: resume for alt button cars
|
||||
pass
|
||||
else:
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL > 0.2:
|
||||
self.button_frame += 1
|
||||
button_counter_offset = [1, 1, 0, None][self.button_frame % 4]
|
||||
if button_counter_offset is not None:
|
||||
for _ in range(20):
|
||||
can_sends.append(hyundaicanfd.create_buttons(packer, self.CP, CAN, (CS.buttons_counter + button_counter_offset) % 0xF, send_button))
|
||||
self.last_button_frame = self.frame
|
||||
|
||||
return can_sends
|
||||
|
||||
def update(self, CS, CC_SP, packer, frame, last_button_frame, CAN) -> list[CanData]:
|
||||
can_sends = []
|
||||
self.CC_SP = CC_SP
|
||||
self.ICBM = CC_SP.intelligentCruiseButtonManagement
|
||||
self.frame = frame
|
||||
self.last_button_frame = last_button_frame
|
||||
|
||||
if self.ICBM.sendButton != SendButtonState.none:
|
||||
send_button = BUTTONS[self.ICBM.sendButton]
|
||||
|
||||
if self.CP.carFingerprint in CANFD_CAR:
|
||||
can_sends.extend(self.create_canfd_mock_button_messages(packer, CS, CAN, send_button))
|
||||
else:
|
||||
can_sends.extend(self.create_can_mock_button_messages(packer, CS, send_button))
|
||||
|
||||
return can_sends
|
||||
@@ -0,0 +1,117 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from abc import ABC, abstractmethod
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.hyundai.values import HyundaiFlags
|
||||
|
||||
|
||||
class LeadData(ABC):
|
||||
def __init__(self, object_gap: int, lead_distance: float, lead_rel_speed: float, lead_visible: bool):
|
||||
self.object_gap = object_gap
|
||||
self.lead_distance = lead_distance
|
||||
self.lead_rel_speed = lead_rel_speed
|
||||
self.lead_visible = lead_visible
|
||||
|
||||
@property
|
||||
@abstractmethod
|
||||
def object_rel_gap(self) -> int:
|
||||
raise NotImplementedError("Subclasses must implement this method")
|
||||
|
||||
|
||||
class CanLeadData(LeadData):
|
||||
@property
|
||||
def object_rel_gap(self) -> int:
|
||||
return 0 if self.lead_distance == 0 else 2 if self.lead_rel_speed < -0.2 else 1
|
||||
|
||||
|
||||
class CanFdLeadData(LeadData):
|
||||
@property
|
||||
def object_rel_gap(self) -> int:
|
||||
return 0 if not self.lead_visible else 2 if self.lead_rel_speed < 0 else 1
|
||||
|
||||
|
||||
def _hysteresis_update(current, new_value, counter, threshold):
|
||||
"""
|
||||
Updates a value based on a hysteresis threshold mechanism. This function
|
||||
compares a new value against the current value and uses a counter to detect
|
||||
when a transition should occur, avoiding rapid oscillations between states.
|
||||
A new value will only be adopted if it differs from the current value and
|
||||
the counter reaches the specified threshold.
|
||||
|
||||
:param current: The current value being tracked.
|
||||
:param new_value: The potential new value to compare against the current.
|
||||
:param counter: The count of consecutive different values encountered.
|
||||
:param threshold: The minimum count required before switching to the new value.
|
||||
:return: A tuple containing:
|
||||
- The updated current value, which is either the original current
|
||||
value or the new value if the hysteresis condition was met.
|
||||
- The updated counter, reset to 0 if the new value was adopted,
|
||||
or incremented by 1 otherwise.
|
||||
"""
|
||||
if new_value == current:
|
||||
return current, 0
|
||||
counter += 1
|
||||
return (new_value, 0) if counter >= threshold else (current, counter)
|
||||
|
||||
|
||||
class LeadDataCarController:
|
||||
# Hysteresis parameters
|
||||
LEAD_HYSTERESIS_FRAMES: int = 50
|
||||
|
||||
def __init__(self, CP: structs.CarParams):
|
||||
self.CP = CP
|
||||
|
||||
self.lead_one = {}
|
||||
self.lead_two = {}
|
||||
|
||||
self._lead_on_counter = 0
|
||||
self._lead_off_counter = 0
|
||||
self.lead_visible = False
|
||||
self.gap_counter = 0
|
||||
self.object_gap = 0
|
||||
self.lead_distance = 0
|
||||
self.lead_rel_speed = 0
|
||||
|
||||
def _update_object_gap(self, lead_distance: float | None):
|
||||
new_gap = 5 # Default gap value if no lead distance is provided
|
||||
if lead_distance is None or lead_distance == 0:
|
||||
new_gap = 0
|
||||
elif lead_distance < 20:
|
||||
new_gap = 2
|
||||
elif lead_distance < 25:
|
||||
new_gap = 3
|
||||
elif lead_distance < 30:
|
||||
new_gap = 4
|
||||
|
||||
self.object_gap, self.gap_counter = _hysteresis_update(self.object_gap, new_gap, self.gap_counter, self.LEAD_HYSTERESIS_FRAMES)
|
||||
|
||||
def _update_lead_visible_hysteresis(self, raw_lead_visible: bool):
|
||||
counter = self._lead_on_counter if raw_lead_visible else self._lead_off_counter
|
||||
self.lead_visible, counter = _hysteresis_update(self.lead_visible, raw_lead_visible, counter, self.LEAD_HYSTERESIS_FRAMES)
|
||||
|
||||
if raw_lead_visible:
|
||||
self._lead_on_counter = counter
|
||||
self._lead_off_counter = 0 # reset opposite counter
|
||||
else:
|
||||
self._lead_off_counter = counter
|
||||
self._lead_on_counter = 0 # reset opposite counter
|
||||
|
||||
def update(self, CC_SP: structs.CarControlSP) -> None:
|
||||
self.lead_one = CC_SP.leadOne
|
||||
self.lead_two = CC_SP.leadTwo
|
||||
|
||||
self.lead_distance = self.lead_one.dRel
|
||||
self.lead_rel_speed = self.lead_one.vRel
|
||||
self._update_lead_visible_hysteresis(self.lead_one.status)
|
||||
self._update_object_gap(self.lead_distance)
|
||||
|
||||
@property
|
||||
def lead_data(self) -> CanLeadData | CanFdLeadData:
|
||||
if self.CP.flags & HyundaiFlags.CANFD:
|
||||
return CanFdLeadData(self.object_gap, self.lead_distance, self.lead_rel_speed, self.lead_visible)
|
||||
|
||||
return CanLeadData(self.object_gap, self.lead_distance, self.lead_rel_speed, self.lead_visible)
|
||||
@@ -0,0 +1,54 @@
|
||||
# **How custom longitudinal tuning works for Hyundai/Kia/Genesis vehicles in sunnypilot.**
|
||||
|
||||
This document explains the custom longitudinal control tuning implemented in sunnypilot for Hyundai, Kia, and Genesis vehicles.
|
||||
|
||||
**Safety First: ISO 15622:2018**
|
||||
|
||||
Our primary safety guideline is [ISO 15622:2018](https://www.iso.org/obp/ui/en/#iso:std:iso:15622:ed-3:v1:en).
|
||||
This standard provides the foundation for the safety limits enforced by this tuning, particularly concerning maximum allowable jerk based on speed.
|
||||
|
||||
**Core Concepts**
|
||||
|
||||
1. **Jerk-Limited Acceleration:** Instead of directly applying the planner's target acceleration (`CC.actuators.accel`), we use a jerk-limited integrator. This ensures smoother transitions by limiting the rate of change of acceleration (jerk).
|
||||
* `desired_accel`: The target acceleration from the planner, clipped within safe limits (`CarControllerParams.ACCEL_MIN`, `CarControllerParams.ACCEL_MAX`). During stopping, this is forced to 0.
|
||||
* `actual_accel`: The acceleration command sent to the car. It's calculated by integrating `desired_accel` but limiting the change from the previous step (`state.accel_last`) based on calculated `jerk_upper` and `jerk_lower` limits.
|
||||
* `jerk_limited_integrator(desired_accel, last_accel, jerk_upper, jerk_lower)`: This function calculates the maximum allowable acceleration change per control step (`DT_CTRL * 2`) based on the upper and lower jerk limits and applies it using `rate_limit`.
|
||||
|
||||
2. **Predictive Jerk Limits:** The upper and lower jerk limits (`jerk_upper`, `jerk_lower`) are not fixed but calculated dynamically based on several factors:
|
||||
* **Actual Acceleration (`aego`):** The car's current acceleration (`CS.out.aEgo`, potentially blended with `CS.aBasis` at low speeds) is smoothed using a `FirstOrderFilter` (`self.aego`).
|
||||
* **Acceleration Error:** The difference between the *commanded* acceleration (`self.accel_cmd`) and the *actual* smoothed acceleration (`self.aego.x`).
|
||||
* **Lookahead Jerk:** We estimate the jerk required to reach the target acceleration (`accel_cmd`) within a future time window. This window (`future_t_upper`, `future_t_lower`) varies with speed, defined in `config.py` (`lookahead_jerk_bp`, `lookahead_jerk_upper_v`, `lookahead_jerk_lower_v`).
|
||||
* `j_ego_upper = accel_error / future_t_upper`
|
||||
* `j_ego_lower = accel_error / future_t_lower`
|
||||
* **ISO 15622 Speed Factors:** Maximum allowed jerk decreases with speed according to ISO 15622. We interpolate values based on current velocity (`CS.out.vEgo`) to get `upper_speed_factor` and `lower_speed_factor`.
|
||||
* **Minimum Jerk (`MIN_JERK`):** A baseline minimum jerk (currently 0.5 m/s³) is enforced to ensure responsiveness.
|
||||
* **Combining Factors:**
|
||||
* `desired_jerk_upper = min(max(j_ego_upper, MIN_JERK), upper_speed_factor)`
|
||||
* `desired_jerk_lower = min(max(-j_ego_lower, MIN_JERK), lower_speed_factor)` (Note: `j_ego_lower` is negated for the lower limit calculation).
|
||||
|
||||
3. **Dynamic Braking Tune:** For use of the tune without the `LONG_TUNING_PREDICTIVE` flag, a specific negative jerk profile is applied during braking (`gen1_accel_error < 0`).
|
||||
* `gen1_accel_error = a_ego_blended - self.state.accel_last`
|
||||
* A dynamic lower jerk limit (`gen1_lower_jerk`) is interpolated based on this error using `DYNAMIC_LOWER_JERK_BP` and a dynamically scaled `DYNAMIC_LOWER_JERK_V` (scaled by `car_config.jerk_limits`).
|
||||
* `gen1_desired_jerk_lower = min(gen1_lower_jerk, lower_speed_factor)`
|
||||
|
||||
4. **Applying Jerk Limits:**
|
||||
* The `jerk_upper` limit is always smoothed using `ramp_update` to prevent abrupt changes.
|
||||
* The `jerk_lower` limit is applied differently based on the `LONG_TUNING_PREDICTIVE` flag:
|
||||
* **If Flag Set (predictive tune):** `jerk_lower` is set directly to `desired_jerk_lower`.
|
||||
* **If Flag Not Set (dynamic tune):** `jerk_lower` is smoothed using `ramp_update` towards `gen1_desired_jerk_lower`.
|
||||
|
||||
**Configuration (`config.py`)**
|
||||
|
||||
* The `CarTuningConfig` dataclass holds tuning parameters like stopping/starting speeds, lookahead jerk profiles, actuator delay, and base jerk limits.
|
||||
* `TUNING_CONFIGS` provides default configurations based on car type (CANFD, EV, HYBRID, DEFAULT).
|
||||
* `CAR_SPECIFIC_CONFIGS` allows overriding defaults for specific car models (e.g., `KIA_NIRO_EV`, `HYUNDAI_IONIQ`).
|
||||
* The `get_car_config` helper function selects the appropriate configuration based on the car fingerprint and flags.
|
||||
* `get_longitudinal_tune` applies relevant config values (like stopping/starting speeds, delay) to the main `CarParams` (CP).
|
||||
|
||||
**Summary**
|
||||
|
||||
This tuning aims for smoother and more responsive longitudinal control by:
|
||||
* Using a jerk-limited integrator for acceleration commands.
|
||||
* Dynamically calculating jerk limits based on lookahead predictions, actual acceleration, and ISO 15622 speed constraints.
|
||||
* Providing specific tuning profiles via a configuration system based on car type and model.
|
||||
* Applying special braking logic for use of predictive or dynamic tune.
|
||||
Whitespace-only changes.
@@ -0,0 +1,55 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from opendbc.car.hyundai.values import CAR
|
||||
|
||||
|
||||
@dataclass
|
||||
class CarTuningConfig:
|
||||
v_ego_stopping: float = 0.25
|
||||
v_ego_starting: float = 0.10
|
||||
stopping_decel_rate: float = 0.40
|
||||
lookahead_jerk_bp: list[float] = field(default_factory=lambda: [2., 5., 20.])
|
||||
lookahead_jerk_upper_v: list[float] = field(default_factory=lambda: [0.3, 0.45, 0.6])
|
||||
lookahead_jerk_lower_v: list[float] = field(default_factory=lambda: [0.3, 0.45, 0.6])
|
||||
longitudinal_actuator_delay: float = 0.50
|
||||
jerk_limits: float = 4.0
|
||||
|
||||
|
||||
# Default configurations for different car types
|
||||
TUNING_CONFIGS = {
|
||||
"CANFD": CarTuningConfig(
|
||||
v_ego_stopping=0.30,
|
||||
),
|
||||
"EV": CarTuningConfig(
|
||||
stopping_decel_rate=0.45,
|
||||
v_ego_stopping=0.35,
|
||||
),
|
||||
"HYBRID": CarTuningConfig(
|
||||
v_ego_starting=0.15,
|
||||
stopping_decel_rate=0.45,
|
||||
v_ego_stopping=0.4,
|
||||
),
|
||||
"DEFAULT": CarTuningConfig(
|
||||
v_ego_stopping=0.3,
|
||||
)
|
||||
}
|
||||
|
||||
# Car-specific configs
|
||||
CAR_SPECIFIC_CONFIGS = {
|
||||
CAR.KIA_NIRO_EV: CarTuningConfig(
|
||||
v_ego_stopping=0.1,
|
||||
stopping_decel_rate=0.3,
|
||||
jerk_limits=3.3,
|
||||
),
|
||||
CAR.KIA_NIRO_PHEV_2022: CarTuningConfig(
|
||||
stopping_decel_rate=0.8,
|
||||
jerk_limits=5.0,
|
||||
),
|
||||
}
|
||||
@@ -0,0 +1,266 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from dataclasses import dataclass
|
||||
|
||||
from opendbc.car import structs, DT_CTRL
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.hyundai.values import CarControllerParams
|
||||
from opendbc.sunnypilot.car.hyundai.longitudinal.helpers import get_car_config, jerk_limited_integrator
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
|
||||
MIN_JERK = 0.5
|
||||
|
||||
|
||||
@dataclass
|
||||
class LongitudinalState:
|
||||
desired_accel: float = 0.0
|
||||
actual_accel: float = 0.0
|
||||
accel_last: float = 0.0
|
||||
jerk_upper: float = 0.0
|
||||
jerk_lower: float = 0.0
|
||||
comfort_band_upper: float = 0.0
|
||||
comfort_band_lower: float = 0.0
|
||||
stopping: bool = False
|
||||
|
||||
|
||||
class LongitudinalController:
|
||||
"""Longitudinal controller which gets injected into CarControllerParams."""
|
||||
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> None:
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
self.tuning = LongitudinalState()
|
||||
self.car_config = get_car_config(CP)
|
||||
self.long_control_state_last = LongCtrlState.off
|
||||
self.stopping_count = 0
|
||||
|
||||
self.accel_cmd = 0.0
|
||||
self.desired_accel = 0.0
|
||||
self.actual_accel = 0.0
|
||||
self.accel_last = 0.0
|
||||
self.jerk_upper = 0.0
|
||||
self.jerk_lower = 0.0
|
||||
self.comfort_band_upper = 0.0
|
||||
self.comfort_band_lower = 0.0
|
||||
self.stopping = False
|
||||
|
||||
@property
|
||||
def enabled(self) -> bool:
|
||||
return bool(self.CP_SP.flags & (HyundaiFlagsSP.LONG_TUNING_DYNAMIC | HyundaiFlagsSP.LONG_TUNING_PREDICTIVE))
|
||||
|
||||
def get_stopping_state(self, actuators: structs.CarControl.Actuators) -> None:
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
|
||||
# If custom tuning is not enabled, use upstream stopping logic
|
||||
if not self.enabled:
|
||||
self.stopping = stopping
|
||||
self.stopping_count = 0
|
||||
return
|
||||
|
||||
# Reset stopping state when not in stopping mode
|
||||
if not stopping:
|
||||
self.stopping = False
|
||||
self.stopping_count = 0
|
||||
return
|
||||
|
||||
# When transitioning from off state to stopping
|
||||
if self.long_control_state_last == LongCtrlState.off:
|
||||
self.stopping = True
|
||||
return
|
||||
|
||||
# Keep track of time in stopping state (in control cycles)
|
||||
if self.stopping_count > 1 / (DT_CTRL * 5):
|
||||
self.stopping = True
|
||||
|
||||
self.stopping_count += 1
|
||||
|
||||
def _calculate_speed_based_jerk_limits(self, velocity: float, long_control_state: LongCtrlState) -> tuple[float, float]:
|
||||
"""Calculate jerk limits based on vehicle speed according to ISO 15622:2018.
|
||||
|
||||
Args:
|
||||
velocity: Current vehicle speed (m/s)
|
||||
long_control_state: Current longitudinal control state
|
||||
|
||||
Returns:
|
||||
Tuple of (upper_limit, lower_limit) in m/s³
|
||||
"""
|
||||
|
||||
if long_control_state == LongCtrlState.pid:
|
||||
upper_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [2.0, 3.0, 2.0]))
|
||||
else:
|
||||
upper_limit = 0.5 # Default for non-PID states
|
||||
|
||||
if self.CP_SP.flags & HyundaiFlagsSP.LONG_TUNING_PREDICTIVE:
|
||||
lower_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [5.0, 3.5, 3.0]))
|
||||
else:
|
||||
lower_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [3.5, 3.5, 3.0]))
|
||||
|
||||
return upper_limit, lower_limit
|
||||
|
||||
def _calculate_lookahead_jerk(self, accel_error: float, velocity: float) -> tuple[float, float]:
|
||||
"""Calculate lookahead jerk needed to reach target acceleration.
|
||||
|
||||
Args:
|
||||
accel_error: Difference between target and current acceleration (m/s²)
|
||||
velocity: Current vehicle speed (m/s)
|
||||
|
||||
Returns:
|
||||
Tuple of (upper_jerk, lower_jerk) in m/s³
|
||||
"""
|
||||
|
||||
# Time window to reach target acceleration, varies with speed
|
||||
future_t_upper = float(np.interp(velocity, self.car_config.lookahead_jerk_bp, self.car_config.lookahead_jerk_upper_v))
|
||||
future_t_lower = float(np.interp(velocity, self.car_config.lookahead_jerk_bp, self.car_config.lookahead_jerk_lower_v))
|
||||
|
||||
# Required jerk to reach target acceleration in lookahead window
|
||||
j_ego_upper = accel_error / future_t_upper
|
||||
j_ego_lower = accel_error / future_t_lower
|
||||
|
||||
jerk_limit = self.car_config.jerk_limits
|
||||
j_ego_upper = np.clip(j_ego_upper, -jerk_limit, jerk_limit)
|
||||
j_ego_lower = np.clip(j_ego_lower, -jerk_limit, jerk_limit)
|
||||
|
||||
return float(j_ego_upper), float(j_ego_lower)
|
||||
|
||||
def calculate_jerk(self, CC: structs.CarControl, CS: CarStateBase, long_control_state: LongCtrlState) -> None:
|
||||
"""Calculate appropriate jerk limits for smooth acceleration/deceleration.
|
||||
|
||||
Args:
|
||||
CC: Car control signals
|
||||
CS: Car state
|
||||
long_control_state: Current longitudinal control state
|
||||
"""
|
||||
|
||||
# If custom tuning is disabled, use upstream fixed values
|
||||
if not self.enabled:
|
||||
jerk_limit = 3.0 if long_control_state == LongCtrlState.pid else 1.0
|
||||
self.jerk_upper = jerk_limit
|
||||
self.jerk_lower = 5.0
|
||||
return
|
||||
|
||||
velocity = CS.out.vEgo
|
||||
accel_error = self.accel_cmd - self.accel_last
|
||||
|
||||
# Calculate jerk limits based on speed
|
||||
upper_speed_factor, lower_speed_factor = self._calculate_speed_based_jerk_limits(velocity, long_control_state)
|
||||
|
||||
# Apply jerk limits based on tuning approach
|
||||
# Predictive tuning uses calculated desired jerk directly
|
||||
# Minimal Dynamic tuning applies only a speed based approach
|
||||
if self.CP_SP.flags & HyundaiFlagsSP.LONG_TUNING_PREDICTIVE:
|
||||
j_ego_upper, j_ego_lower = self._calculate_lookahead_jerk(accel_error, velocity)
|
||||
desired_jerk_upper = min(max(j_ego_upper, MIN_JERK), upper_speed_factor)
|
||||
desired_jerk_lower = min(max(-j_ego_lower, MIN_JERK), lower_speed_factor)
|
||||
|
||||
self.jerk_upper = desired_jerk_upper
|
||||
self.jerk_lower = desired_jerk_lower if not self.CP.radarUnavailable else 5.0
|
||||
else:
|
||||
self.jerk_upper = upper_speed_factor
|
||||
self.jerk_lower = lower_speed_factor
|
||||
|
||||
# Disable jerk when longitudinal control is inactive
|
||||
if not CC.longActive:
|
||||
self.jerk_upper = 0.0
|
||||
self.jerk_lower = 0.0
|
||||
|
||||
def calculate_accel(self, CC: structs.CarControl) -> None:
|
||||
"""Calculate commanded acceleration using jerk-limited approach.
|
||||
|
||||
Args:
|
||||
CC: Car control signals
|
||||
"""
|
||||
|
||||
# Skip custom processing if tuning is disabled or radar unavailable
|
||||
if not self.enabled:
|
||||
self.desired_accel = self.accel_cmd
|
||||
self.actual_accel = self.accel_cmd
|
||||
return
|
||||
|
||||
# Reset acceleration when control is inactive
|
||||
if not CC.longActive:
|
||||
self.desired_accel = 0.0
|
||||
self.actual_accel = 0.0
|
||||
self.accel_last = 0.0
|
||||
return
|
||||
|
||||
# Force zero acceleration during stopping
|
||||
if self.stopping:
|
||||
self.desired_accel = 0.0
|
||||
else:
|
||||
self.desired_accel = float(np.clip(self.accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
|
||||
# Apply jerk-limited integration to get smooth acceleration
|
||||
if not self.CP.radarUnavailable:
|
||||
self.actual_accel = jerk_limited_integrator(self.desired_accel, self.accel_last, self.jerk_upper, self.jerk_lower)
|
||||
else:
|
||||
self.actual_accel = self.desired_accel
|
||||
|
||||
self.accel_last = self.actual_accel
|
||||
|
||||
def calculate_comfort_band(self, CC: structs.CarControl, CS: CarStateBase) -> None:
|
||||
if not self.enabled or not CC.longActive:
|
||||
self.comfort_band_upper = 0.0
|
||||
self.comfort_band_lower = 0.0
|
||||
return
|
||||
|
||||
accel = CS.out.aEgo
|
||||
accel_vals = [0.0, 0.3, 0.6, 0.9, 1.2, 2.0]
|
||||
decel_vals = [-3.5, -2.5, -1.5, -1.0, -0.5, -0.05]
|
||||
comfort_band_vals = [0.0, 0.02, 0.04, 0.06, 0.08, 0.10]
|
||||
|
||||
if self.CP_SP.flags & HyundaiFlagsSP.LONG_TUNING_PREDICTIVE:
|
||||
self.comfort_band_upper = float(np.interp(accel, accel_vals, comfort_band_vals))
|
||||
self.comfort_band_lower = float(np.interp(accel, decel_vals, comfort_band_vals[::-1]))
|
||||
else:
|
||||
self.comfort_band_upper = float(np.interp(accel, [0.0, 2.0], [0.0, 0.04]))
|
||||
self.comfort_band_lower = float(np.interp(accel, [-3.5, -0.05], [0.04, 0.0]))
|
||||
|
||||
def get_tuning_state(self) -> None:
|
||||
"""Update the tuning state object with current control values.
|
||||
|
||||
External components depend on this state for longitudinal control.
|
||||
"""
|
||||
|
||||
self.tuning = LongitudinalState(
|
||||
desired_accel=self.desired_accel,
|
||||
actual_accel=self.actual_accel,
|
||||
accel_last=self.accel_last,
|
||||
jerk_upper=self.jerk_upper,
|
||||
jerk_lower=self.jerk_lower,
|
||||
comfort_band_upper=self.comfort_band_upper,
|
||||
comfort_band_lower=self.comfort_band_lower,
|
||||
stopping=self.stopping,
|
||||
)
|
||||
|
||||
def update(self, CC: structs.CarControl, CS: CarStateBase) -> None:
|
||||
"""Update longitudinal control calculations.
|
||||
|
||||
This is the main entry point called externally.
|
||||
|
||||
Args:
|
||||
CC: Car control signals including actuators
|
||||
CS: Car state information
|
||||
"""
|
||||
|
||||
actuators = CC.actuators
|
||||
long_control_state = actuators.longControlState
|
||||
self.accel_cmd = CC.actuators.accel
|
||||
|
||||
self.get_stopping_state(actuators)
|
||||
self.calculate_jerk(CC, CS, long_control_state)
|
||||
self.calculate_accel(CC)
|
||||
self.calculate_comfort_band(CC, CS)
|
||||
self.get_tuning_state()
|
||||
|
||||
self.long_control_state_last = long_control_state
|
||||
|
||||
# TODO-SP Why were we calculating both jerks at once lol
|
||||
@@ -0,0 +1,53 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from opendbc.car import structs, DT_CTRL, rate_limit
|
||||
from opendbc.car.hyundai.values import HyundaiFlags
|
||||
from opendbc.sunnypilot.car.hyundai.longitudinal.config import CarTuningConfig, TUNING_CONFIGS, CAR_SPECIFIC_CONFIGS
|
||||
|
||||
JERK_THRESHOLD = 0.1
|
||||
JERK_STEP = 0.1
|
||||
|
||||
|
||||
class LongitudinalTuningType:
|
||||
OFF = 0
|
||||
DYNAMIC = 1
|
||||
PREDICTIVE = 2
|
||||
|
||||
|
||||
def get_car_config(CP: structs.CarParams) -> CarTuningConfig:
|
||||
# Get car type flags from specific configs or determine from car flags
|
||||
car_config = CAR_SPECIFIC_CONFIGS.get(CP.carFingerprint)
|
||||
# If car is not in specific configs, determine from flags
|
||||
if car_config is None:
|
||||
if CP.flags & HyundaiFlags.CANFD:
|
||||
car_config = TUNING_CONFIGS["CANFD"]
|
||||
elif CP.flags & HyundaiFlags.EV:
|
||||
car_config = TUNING_CONFIGS["EV"]
|
||||
elif CP.flags & HyundaiFlags.HYBRID:
|
||||
car_config = TUNING_CONFIGS["HYBRID"]
|
||||
else:
|
||||
car_config = TUNING_CONFIGS["DEFAULT"]
|
||||
|
||||
return car_config
|
||||
|
||||
|
||||
def get_longitudinal_tune(CP: structs.CarParams) -> None:
|
||||
config = get_car_config(CP)
|
||||
CP.vEgoStopping = config.v_ego_stopping
|
||||
CP.vEgoStarting = config.v_ego_starting
|
||||
CP.stoppingDecelRate = config.stopping_decel_rate
|
||||
CP.startingState = False
|
||||
CP.longitudinalActuatorDelay = config.longitudinal_actuator_delay
|
||||
|
||||
|
||||
def jerk_limited_integrator(desired_accel, last_accel, jerk_upper, jerk_lower) -> float:
|
||||
if desired_accel >= last_accel:
|
||||
val = jerk_upper * DT_CTRL * 5
|
||||
else:
|
||||
val = jerk_lower * DT_CTRL * 5
|
||||
|
||||
return rate_limit(desired_accel, last_accel, -val, val)
|
||||
@@ -0,0 +1,108 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from enum import StrEnum
|
||||
from collections import namedtuple
|
||||
|
||||
from opendbc.car import Bus, DT_CTRL, structs
|
||||
from opendbc.car.hyundai.values import CAR
|
||||
|
||||
from opendbc.car.hyundai.values import HyundaiFlags
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
from opendbc.sunnypilot.mads_base import MadsCarStateBase
|
||||
from opendbc.can.parser import CANParser
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
|
||||
MadsDataSP = namedtuple("MadsDataSP",
|
||||
["enable_mads", "lat_active", "disengaging", "paused"])
|
||||
|
||||
|
||||
class MadsCarController:
|
||||
def __init__(self):
|
||||
self.mads = MadsDataSP(False, False, False, False)
|
||||
|
||||
self.lat_disengage_blink = 0
|
||||
self.lat_disengage_init = False
|
||||
self.prev_lat_active = False
|
||||
|
||||
self.lkas_icon = 0
|
||||
self.lfa_icon = 0
|
||||
|
||||
# display LFA "white_wheel" and LKAS "White car + lanes" when not CC.latActive
|
||||
def mads_status_update(self, CC: structs.CarControl, CC_SP: structs.CarControlSP, frame: int) -> MadsDataSP:
|
||||
enable_mads = CC_SP.mads.available
|
||||
|
||||
if CC.latActive:
|
||||
self.lat_disengage_init = False
|
||||
elif self.prev_lat_active:
|
||||
self.lat_disengage_init = True
|
||||
|
||||
if not self.lat_disengage_init:
|
||||
self.lat_disengage_blink = frame
|
||||
|
||||
paused = CC_SP.mads.enabled and not CC.latActive
|
||||
disengaging = (frame - self.lat_disengage_blink) * DT_CTRL < 1.0 if self.lat_disengage_init else False
|
||||
|
||||
self.prev_lat_active = CC.latActive
|
||||
|
||||
return MadsDataSP(enable_mads, CC.latActive, disengaging, paused)
|
||||
|
||||
def create_lkas_icon(self, CP: structs.CarParams, enabled: bool) -> int:
|
||||
if self.mads.enable_mads:
|
||||
lkas_icon = 2 if self.mads.lat_active else 3 if self.mads.disengaging else 1
|
||||
else:
|
||||
lkas_icon = 2 if enabled else 1
|
||||
|
||||
# Override common signals for KIA_OPTIMA_G4 and KIA_OPTIMA_G4_FL
|
||||
if CP.carFingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.HYUNDAI_KONA_NON_SCC):
|
||||
lkas_icon = 3 if (self.mads.lat_active if self.mads.enable_mads else enabled) else 1
|
||||
|
||||
return lkas_icon
|
||||
|
||||
def create_lfa_icon(self, enabled: bool) -> int:
|
||||
if self.mads.enable_mads:
|
||||
lfa_icon = 2 if self.mads.lat_active else 3 if self.mads.disengaging else 1 if self.mads.paused else 0
|
||||
else:
|
||||
lfa_icon = 2 if enabled else 0
|
||||
|
||||
return lfa_icon
|
||||
|
||||
def update(self, CP: structs.CarParams, CC: structs.CarControl, CC_SP: structs.CarControlSP, frame: int) -> None:
|
||||
self.mads = self.mads_status_update(CC, CC_SP, frame)
|
||||
self.lkas_icon = self.create_lkas_icon(CP, CC.enabled)
|
||||
self.lfa_icon = self.create_lfa_icon(CC.enabled)
|
||||
|
||||
|
||||
class MadsCarState(MadsCarStateBase):
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParams):
|
||||
super().__init__(CP, CP_SP)
|
||||
self.main_cruise_enabled: bool = False
|
||||
|
||||
@staticmethod
|
||||
def get_parser(CP, CP_SP, pt_messages) -> None:
|
||||
pass
|
||||
|
||||
def get_main_cruise(self, ret: structs.CarState) -> bool:
|
||||
if self.CP_SP.flags & HyundaiFlagsSP.LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE:
|
||||
if any(be.type == ButtonType.mainCruise and be.pressed for be in ret.buttonEvents):
|
||||
self.main_cruise_enabled = not self.main_cruise_enabled
|
||||
else:
|
||||
self.main_cruise_enabled = True
|
||||
|
||||
return self.main_cruise_enabled if ret.cruiseState.available else False
|
||||
|
||||
def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
pass
|
||||
|
||||
def update_mads_canfd(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp
|
||||
ret.cruiseState.available = cp_cruise_info.vl["SCC_CONTROL"]["MainMode_ACC"] == 1
|
||||
@@ -0,0 +1,82 @@
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.car import structs, Bus
|
||||
from opendbc.car.hyundai.hyundaicanfd import CanBus
|
||||
from opendbc.car.hyundai.values import DBC, HyundaiFlags
|
||||
|
||||
from opendbc.sunnypilot.car.hyundai.escc import EsccRadarInterfaceBase
|
||||
|
||||
|
||||
class RadarInterfaceExt(EsccRadarInterfaceBase):
|
||||
msg_src: str
|
||||
trigger_msg: int
|
||||
rcp: CANParser
|
||||
pts: dict[int, structs.RadarData.RadarPoint]
|
||||
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
EsccRadarInterfaceBase.__init__(self, CP, CP_SP)
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
self.track_id = 0
|
||||
|
||||
@property
|
||||
def use_radar_interface_ext(self) -> bool:
|
||||
return self.use_escc or self.CP.flags & (HyundaiFlags.CAMERA_SCC | HyundaiFlags.CANFD_CAMERA_SCC)
|
||||
|
||||
def get_msg_src(self) -> str | None:
|
||||
if self.use_escc:
|
||||
return "ESCC"
|
||||
if self.CP.flags & (HyundaiFlags.CAMERA_SCC | HyundaiFlags.CANFD_CAMERA_SCC):
|
||||
return "SCC_CONTROL" if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else "SCC11"
|
||||
|
||||
def get_radar_ext_can_parser(self) -> CANParser:
|
||||
if self.ESCC.enabled:
|
||||
lead_src, bus = "ESCC", 0
|
||||
elif self.CP.flags & (HyundaiFlags.CAMERA_SCC | HyundaiFlags.CANFD_CAMERA_SCC):
|
||||
lead_src = "SCC_CONTROL" if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else "SCC11"
|
||||
bus = CanBus(self.CP).CAM if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else 2
|
||||
else:
|
||||
return None
|
||||
|
||||
messages = [(lead_src, 50)]
|
||||
return CANParser(DBC[self.CP.carFingerprint][Bus.pt], messages, bus)
|
||||
|
||||
def get_trigger_msg(self, default_trigger_msg) -> int:
|
||||
if self.ESCC.enabled:
|
||||
return self.ESCC.trigger_msg
|
||||
if self.CP.flags & (HyundaiFlags.CAMERA_SCC | HyundaiFlags.CANFD_CAMERA_SCC):
|
||||
return 0x1A0 if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else 0x420
|
||||
return default_trigger_msg
|
||||
|
||||
def initialize_radar_ext(self, default_trigger_msg) -> None:
|
||||
if self.ESCC.enabled:
|
||||
self.use_escc = True
|
||||
|
||||
self.rcp = self.get_radar_ext_can_parser()
|
||||
self.trigger_msg = self.get_trigger_msg(default_trigger_msg)
|
||||
|
||||
def update_ext(self, ret: structs.RadarData) -> structs.RadarData:
|
||||
if not self.rcp.can_valid:
|
||||
ret.errors.canError = True
|
||||
return ret
|
||||
|
||||
for ii in range(1):
|
||||
msg_src = self.get_msg_src()
|
||||
msg = self.rcp.vl[msg_src]
|
||||
|
||||
if ii not in self.pts:
|
||||
self.pts[ii] = structs.RadarData.RadarPoint()
|
||||
self.pts[ii].trackId = self.track_id
|
||||
self.track_id += 1
|
||||
|
||||
valid = msg['ACC_ObjDist'] < 204.6 if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else msg['ACC_ObjStatus']
|
||||
if valid:
|
||||
self.pts[ii].dRel = msg['ACC_ObjDist']
|
||||
self.pts[ii].yRel = float('nan') # FIXME-SP: Only some cars have lateral position from SCC
|
||||
self.pts[ii].vRel = msg['ACC_ObjRelSpd']
|
||||
|
||||
else:
|
||||
del self.pts[ii]
|
||||
|
||||
ret.points = list(self.pts.values())
|
||||
return ret
|
||||
Whitespace-only changes.
@@ -0,0 +1,64 @@
|
||||
import unittest
|
||||
|
||||
from opendbc.sunnypilot.car.hyundai.escc import EnhancedSmartCruiseControl, ESCC_MSG
|
||||
from opendbc.car.hyundai.carstate import CarState
|
||||
from opendbc.car import structs
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
|
||||
|
||||
class TestEscc(unittest.TestCase):
|
||||
def _make_car_params(self):
|
||||
params = structs.CarParams()
|
||||
params.carFingerprint = "HYUNDAI_SONATA"
|
||||
return params
|
||||
|
||||
def _make_car_params_sp(self):
|
||||
params = structs.CarParamsSP()
|
||||
params.flags = HyundaiFlagsSP.ENHANCED_SCC
|
||||
return params
|
||||
|
||||
def _make_escc(self):
|
||||
return EnhancedSmartCruiseControl(self._make_car_params(), self._make_car_params_sp())
|
||||
|
||||
def test_escc_msg_id(self):
|
||||
escc = self._make_escc()
|
||||
self.assertEqual(escc.trigger_msg, ESCC_MSG)
|
||||
|
||||
def test_enabled_flag(self):
|
||||
car_params = self._make_car_params()
|
||||
for value in range(256):
|
||||
with self.subTest(flags=value):
|
||||
car_params_sp = structs.CarParamsSP()
|
||||
car_params_sp.flags = value
|
||||
escc = EnhancedSmartCruiseControl(car_params, car_params_sp)
|
||||
self.assertEqual(escc.enabled, bool(value & HyundaiFlagsSP.ENHANCED_SCC))
|
||||
|
||||
def test_update_car_state(self):
|
||||
car_params = self._make_car_params()
|
||||
car_params_sp = self._make_car_params_sp()
|
||||
escc = EnhancedSmartCruiseControl(car_params, car_params_sp)
|
||||
car_state = CarState(car_params, car_params_sp)
|
||||
car_state.escc_cmd_act = 1
|
||||
car_state.escc_aeb_warning = 1
|
||||
car_state.escc_aeb_dec_cmd_act = 1
|
||||
car_state.escc_aeb_dec_cmd = 1
|
||||
escc.update_car_state(car_state)
|
||||
self.assertEqual(escc.car_state, car_state)
|
||||
|
||||
def test_update_scc12(self):
|
||||
car_params = self._make_car_params()
|
||||
car_params_sp = self._make_car_params_sp()
|
||||
escc = EnhancedSmartCruiseControl(car_params, car_params_sp)
|
||||
car_state = CarState(car_params, car_params_sp)
|
||||
car_state.escc_cmd_act = 1
|
||||
car_state.escc_aeb_warning = 1
|
||||
car_state.escc_aeb_dec_cmd_act = 1
|
||||
car_state.escc_aeb_dec_cmd = 1
|
||||
escc.update_car_state(car_state)
|
||||
scc12_message = {}
|
||||
escc.update_scc12(scc12_message)
|
||||
self.assertEqual(scc12_message["AEB_CmdAct"], 1)
|
||||
self.assertEqual(scc12_message["CF_VSM_Warn"], 1)
|
||||
self.assertEqual(scc12_message["CF_VSM_DecCmdAct"], 1)
|
||||
self.assertEqual(scc12_message["CR_VSM_DecCmd"], 1)
|
||||
self.assertEqual(scc12_message["AEB_Status"], 2)
|
||||
@@ -0,0 +1,78 @@
|
||||
import unittest
|
||||
from enum import IntFlag
|
||||
|
||||
from opendbc.sunnypilot.car.hyundai.lead_data_ext import LeadDataCarController, CanLeadData, CanFdLeadData
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.hyundai.values import HyundaiFlags
|
||||
|
||||
|
||||
def make_carparams(flags: IntFlag = HyundaiFlags.LEGACY):
|
||||
cp = structs.CarParams()
|
||||
cp.carFingerprint = "HYUNDAI_SONATA"
|
||||
cp.flags = flags.value
|
||||
return cp
|
||||
|
||||
|
||||
def make_carcontrolsp(leadDistance=10.0, leadRelSpeed=0.0, leadVisible=True):
|
||||
c = structs.CarControlSP()
|
||||
c.leadOne.dRel = leadDistance
|
||||
c.leadOne.vRel = leadRelSpeed
|
||||
c.leadOne.status = leadVisible
|
||||
return c
|
||||
|
||||
|
||||
class TestLeadDataCarController(unittest.TestCase):
|
||||
def test_update_object_gap(self):
|
||||
ctrl = LeadDataCarController(make_carparams())
|
||||
# Initial value should be 0
|
||||
self.assertEqual(ctrl.object_gap, 0)
|
||||
|
||||
# Set to 15 (should become 2 after hysteresis)
|
||||
for _ in range(ctrl.LEAD_HYSTERESIS_FRAMES):
|
||||
ctrl._update_object_gap(15)
|
||||
self.assertEqual(ctrl.object_gap, 2)
|
||||
|
||||
# Set to 22 (should become 3 after hysteresis)
|
||||
for _ in range(ctrl.LEAD_HYSTERESIS_FRAMES):
|
||||
ctrl._update_object_gap(22)
|
||||
self.assertEqual(ctrl.object_gap, 3)
|
||||
|
||||
# Set to 0 (should become 0 after hysteresis)
|
||||
for _ in range(ctrl.LEAD_HYSTERESIS_FRAMES):
|
||||
ctrl._update_object_gap(0)
|
||||
self.assertEqual(ctrl.object_gap, 0)
|
||||
|
||||
def test_update_lead_visible_hysteresis(self):
|
||||
ctrl = LeadDataCarController(make_carparams())
|
||||
ctrl._update_lead_visible_hysteresis(True)
|
||||
self.assertIsInstance(ctrl.lead_visible, bool)
|
||||
ctrl._update_lead_visible_hysteresis(False)
|
||||
self.assertIsInstance(ctrl.lead_visible, bool)
|
||||
|
||||
def test_update(self):
|
||||
ctrl = LeadDataCarController(make_carparams())
|
||||
sp = make_carcontrolsp(leadDistance=25, leadRelSpeed=-0.5, leadVisible=True)
|
||||
ctrl.update(sp)
|
||||
self.assertEqual(ctrl.lead_distance, 25)
|
||||
self.assertEqual(ctrl.lead_rel_speed, -0.5)
|
||||
self.assertIsInstance(ctrl.lead_visible, bool)
|
||||
|
||||
def test_lead_data_can(self):
|
||||
ctrl = LeadDataCarController(make_carparams())
|
||||
ctrl.object_gap = 1
|
||||
ctrl.lead_distance = 10
|
||||
ctrl.lead_rel_speed = -0.3
|
||||
ctrl.lead_visible = True
|
||||
ld = ctrl.lead_data
|
||||
self.assertIsInstance(ld, CanLeadData)
|
||||
self.assertEqual(ld.object_rel_gap, 2)
|
||||
|
||||
def test_lead_data_canfd(self):
|
||||
ctrl = LeadDataCarController(make_carparams(HyundaiFlags.CANFD))
|
||||
ctrl.object_gap = 1
|
||||
ctrl.lead_distance = 10
|
||||
ctrl.lead_rel_speed = 1.0
|
||||
ctrl.lead_visible = True
|
||||
ld = ctrl.lead_data
|
||||
self.assertIsInstance(ld, CanFdLeadData)
|
||||
self.assertEqual(ld.object_rel_gap, 1)
|
||||
@@ -0,0 +1,121 @@
|
||||
import unittest
|
||||
|
||||
from opendbc.testing import parameterized
|
||||
|
||||
from opendbc.car import CanData
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.hyundai.values import CAR, HyundaiFlags
|
||||
from opendbc.sunnypilot.car.hyundai.escc import ESCC_MSG
|
||||
|
||||
ESCC_CARS = [
|
||||
(CAR.HYUNDAI_ELANTRA_2021, ESCC_MSG),
|
||||
]
|
||||
|
||||
CAMERA_SCC_CARS = [
|
||||
(CAR.HYUNDAI_KONA_EV_2022, 0, 0x420, "SCC11"),
|
||||
(CAR.HYUNDAI_IONIQ_5, HyundaiFlags.CANFD_CAMERA_SCC.value, 0x1A0, "SCC_CONTROL"),
|
||||
]
|
||||
|
||||
STANDARD_RADAR_CARS = [
|
||||
(CAR.HYUNDAI_ELANTRA_2021, 0),
|
||||
(CAR.HYUNDAI_SANTA_FE, 0),
|
||||
]
|
||||
|
||||
|
||||
class TestRadarInterfaceExt(unittest.TestCase):
|
||||
|
||||
@staticmethod
|
||||
def _setup_platform(car_name, additional_flags=0, escc_msg=None):
|
||||
"""Set up the platform with specific parameters"""
|
||||
CarInterface = interfaces[car_name]
|
||||
|
||||
CP = CarInterface.get_non_essential_params(car_name)
|
||||
CP.flags |= additional_flags
|
||||
|
||||
CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
|
||||
|
||||
CI = CarInterface(CP, CP_SP)
|
||||
|
||||
RD = CI.RadarInterface(CP, CP_SP)
|
||||
|
||||
if escc_msg is not None and hasattr(RD, 'use_escc'):
|
||||
try:
|
||||
RD.use_escc = True
|
||||
except AttributeError:
|
||||
object.__setattr__(RD, 'use_escc', True)
|
||||
|
||||
return RD, CP, CP_SP
|
||||
|
||||
@parameterized("car_name, escc_msg", ESCC_CARS)
|
||||
def test_escc_radar_interface(self, car_name, escc_msg):
|
||||
"""Test radar interface for ESCC-enabled cars"""
|
||||
RD, CP, CP_SP = self._setup_platform(car_name, escc_msg=escc_msg)
|
||||
|
||||
# Assert that ESCC features are present
|
||||
if hasattr(RD, 'use_escc'):
|
||||
self.assertTrue(RD.use_escc, "ESCC car should have use_escc=True")
|
||||
if hasattr(RD, 'use_radar_interface_ext'):
|
||||
self.assertTrue(RD.use_radar_interface_ext, "ESCC car should use radar interface ext")
|
||||
|
||||
# Run radar interface once
|
||||
RD.update([])
|
||||
|
||||
# Test radar fault
|
||||
if not CP.radarUnavailable and RD.rcp is not None:
|
||||
cans = [(0, [CanData(0, b'', 0) for _ in range(5)])]
|
||||
rr = RD.update(cans)
|
||||
self.assertTrue(rr is None or len(rr.errors) > 0)
|
||||
|
||||
@parameterized("car_name, flags, expected_trigger, msg_src", CAMERA_SCC_CARS)
|
||||
def test_camera_scc_radar_interface(self, car_name, flags, expected_trigger, msg_src):
|
||||
"""Test radar interface for Camera SCC cars"""
|
||||
RD, CP, CP_SP = self._setup_platform(car_name, additional_flags=flags)
|
||||
|
||||
# Assert Camera SCC flag is set appropriately
|
||||
if flags & HyundaiFlags.CAMERA_SCC:
|
||||
self.assertTrue(CP.flags & HyundaiFlags.CAMERA_SCC, "Car should have CAMERA_SCC flag")
|
||||
if flags & HyundaiFlags.CANFD_CAMERA_SCC:
|
||||
self.assertTrue(CP.flags & HyundaiFlags.CANFD_CAMERA_SCC, "Car should have CANFD_CAMERA_SCC flag")
|
||||
|
||||
# Check if using radar interface ext
|
||||
if hasattr(RD, 'use_radar_interface_ext'):
|
||||
self.assertTrue(RD.use_radar_interface_ext, "Camera SCC car should use radar interface ext")
|
||||
|
||||
# Verify trigger message
|
||||
if hasattr(RD, 'trigger_msg'):
|
||||
self.assertEqual(RD.trigger_msg, expected_trigger, f"Expected trigger_msg {expected_trigger}, got {RD.trigger_msg}")
|
||||
|
||||
# Run radar interface once
|
||||
RD.update([])
|
||||
|
||||
# Test radar fault
|
||||
if not CP.radarUnavailable and RD.rcp is not None:
|
||||
cans = [(0, [CanData(0, b'', 0) for _ in range(5)])]
|
||||
rr = RD.update(cans)
|
||||
self.assertTrue(rr is None or len(rr.errors) > 0)
|
||||
|
||||
@parameterized("car_name, flags", STANDARD_RADAR_CARS)
|
||||
def test_standard_radar_interface(self, car_name, flags):
|
||||
"""Test radar interface for standard radar cars"""
|
||||
RD, CP, CP_SP = self._setup_platform(car_name, additional_flags=flags)
|
||||
|
||||
# Standard cars should not use radar interface ext
|
||||
if hasattr(RD, 'use_radar_interface_ext'):
|
||||
self.assertFalse(RD.use_radar_interface_ext, "Standard car should not use radar interface ext")
|
||||
|
||||
# Run radar interface once
|
||||
RD.update([])
|
||||
|
||||
# For standard radar, test the _update method directly if available
|
||||
if not CP.radarUnavailable and RD.rcp is not None and \
|
||||
hasattr(RD, '_update') and hasattr(RD, 'trigger_msg'):
|
||||
# Setup for _update test if needed
|
||||
if hasattr(RD, 'updated_messages'):
|
||||
RD.updated_messages = {RD.trigger_msg}
|
||||
RD._update(RD.updated_messages)
|
||||
|
||||
# Test radar fault
|
||||
if not CP.radarUnavailable and RD.rcp is not None:
|
||||
cans = [(0, [CanData(0, b'', 0) for _ in range(5)])]
|
||||
rr = RD.update(cans)
|
||||
self.assertTrue(rr is None or len(rr.errors) > 0)
|
||||
@@ -0,0 +1,173 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import unittest
|
||||
import numpy as np
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.sunnypilot.car.hyundai.longitudinal.controller import LongitudinalController
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
|
||||
|
||||
@dataclass
|
||||
class CP:
|
||||
carFingerprint: str = "KIA_NIRO_EV"
|
||||
flags: int = 0
|
||||
radarUnavailable: bool = False
|
||||
|
||||
|
||||
@dataclass
|
||||
class Actuators:
|
||||
accel: float = 0.0
|
||||
longControlState = LongCtrlState
|
||||
|
||||
|
||||
@dataclass
|
||||
class CC:
|
||||
actuators: Actuators = field(default_factory=lambda: Actuators())
|
||||
longActive: bool = True
|
||||
|
||||
|
||||
@dataclass
|
||||
class Out:
|
||||
vEgo: float = 0.0
|
||||
aEgo: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class CS:
|
||||
out: Out = field(default_factory=lambda: Out())
|
||||
aBasis: float = 0.0
|
||||
|
||||
|
||||
class TestLongitudinalTuningController(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.CP = CP(flags=0)
|
||||
self.CP_SP = CP(flags=0)
|
||||
self.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_PREDICTIVE
|
||||
self.CS = CS()
|
||||
self.CC = CC()
|
||||
self.controller = LongitudinalController(self.CP, self.CP_SP)
|
||||
|
||||
def test_enabled_and_disabled(self):
|
||||
self.assertTrue(self.controller.enabled)
|
||||
self.CP_SP.flags = 0
|
||||
self.assertFalse(self.controller.enabled)
|
||||
|
||||
def test_stopping_state(self):
|
||||
self.CC.actuators.longControlState = LongCtrlState.stopping
|
||||
self.controller.get_stopping_state(self.CC.actuators)
|
||||
self.assertTrue(self.controller.stopping)
|
||||
self.assertEqual(self.controller.stopping_count, 0)
|
||||
self.CC.actuators.longControlState = LongCtrlState.pid
|
||||
self.controller.get_stopping_state(self.CC.actuators)
|
||||
self.assertFalse(self.controller.stopping)
|
||||
|
||||
def test_calc_speed_based_jerk(self):
|
||||
self.assertEqual((0.5, 5.0), self.controller._calculate_speed_based_jerk_limits(0.0, LongCtrlState.stopping))
|
||||
velocities = [0.0, 2.0, 5.0, 7.0, 10.0, 15.0, 20.0, 25.0, 30.0]
|
||||
|
||||
for velocity in velocities:
|
||||
upper_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [2.0, 3.0, 2.0]))
|
||||
lower_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [5.0, 3.5, 3.0]))
|
||||
expected = (upper_limit, lower_limit)
|
||||
actual = self.controller._calculate_speed_based_jerk_limits(velocity, LongCtrlState.pid)
|
||||
self.assertEqual(expected, actual)
|
||||
|
||||
def test_calc_lookahead_jerk(self):
|
||||
upper, lower = self.controller._calculate_lookahead_jerk(-0.5, 4.9)
|
||||
self.assertAlmostEqual(upper, -1.1, delta=0.1)
|
||||
self.assertAlmostEqual(lower, -1.1, delta=0.1)
|
||||
upper, lower = self.controller._calculate_lookahead_jerk(0.5, 5.0)
|
||||
self.assertAlmostEqual(upper, 1.1, delta=0.1)
|
||||
self.assertAlmostEqual(lower, 1.1, delta=0.1)
|
||||
|
||||
def test_calc_jerk(self):
|
||||
self.CP_SP.flags = 0
|
||||
self.controller.calculate_jerk(self.CC, self.CS, LongCtrlState.pid)
|
||||
self.assertEqual(self.controller.jerk_upper, 3.0)
|
||||
self.assertEqual(self.controller.jerk_lower, 5.0)
|
||||
self.controller.calculate_jerk(self.CC, self.CS, LongCtrlState.off)
|
||||
self.assertEqual(self.controller.jerk_upper, 1.0)
|
||||
|
||||
self.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_PREDICTIVE
|
||||
self.controller.__init__(self.CP, self.CP_SP)
|
||||
self.CS.out.vEgo = 10.0
|
||||
self.controller.accel_cmd = -3.5
|
||||
self.controller.accel_last = -1.0
|
||||
self.controller.calculate_jerk(self.CC, self.CS, LongCtrlState.pid)
|
||||
self.assertEqual(self.controller.jerk_upper, 0.5)
|
||||
self.assertAlmostEqual(self.controller.jerk_lower, 3.3, delta=0.01)
|
||||
|
||||
self.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_DYNAMIC
|
||||
self.controller.__init__(self.CP, self.CP_SP)
|
||||
self.CS.out.vEgo = 10.0
|
||||
self.CS.aBasis = -3.3
|
||||
self.CS.out.aEgo = -3.5
|
||||
self.controller.accel_cmd = -3.5
|
||||
self.controller.accel_last = -1.0
|
||||
for _ in range(50):
|
||||
self.controller.calculate_jerk(self.CC, self.CS, LongCtrlState.pid)
|
||||
self.assertAlmostEqual(self.controller.jerk_upper, 2.67, delta=0.01)
|
||||
self.assertAlmostEqual(self.controller.jerk_lower, 3.33, delta=0.01)
|
||||
|
||||
def test_calc_accel(self):
|
||||
self.CP_SP.flags = 0
|
||||
self.controller.accel_cmd = 1.5
|
||||
self.controller.calculate_accel(self.CC)
|
||||
self.assertEqual(self.controller.desired_accel, self.controller.accel_cmd)
|
||||
|
||||
self.CP_SP.flags = HyundaiFlagsSP.LONG_TUNING_DYNAMIC
|
||||
self.CC.longActive = False
|
||||
self.controller.calculate_accel(self.CC)
|
||||
self.assertEqual(self.controller.desired_accel, 0.0)
|
||||
|
||||
self.CC.longActive = True
|
||||
self.controller.stopping = True
|
||||
self.controller.calculate_accel(self.CC)
|
||||
self.assertEqual(self.controller.desired_accel, 0.0)
|
||||
|
||||
def test_calc_comfort_band(self):
|
||||
stock_decels_list = [-3.5, -2.5, -1.5, -1.0, -0.5, -0.05]
|
||||
stock_accels_list = [0.0, 0.3, 0.6, 0.9, 1.2, 2.0]
|
||||
stock_comfort_band_vals = [0.0, 0.02, 0.04, 0.06, 0.08, 0.10]
|
||||
|
||||
decels_list = [-3.5, -3.1, -2.245, -1.853, -1.234, -0.64352, -0.06432, -0.00005]
|
||||
accels_list = [0.0, 0.23345, 0.456, 0.5677, 0.6788, 0.834, 1.0, 1.3456, 1.8]
|
||||
|
||||
for decel in decels_list:
|
||||
self.CS.out.aEgo = decel
|
||||
self.controller.calculate_comfort_band(self.CC, self.CS)
|
||||
actual = self.controller.comfort_band_lower
|
||||
expected = float(np.interp(decel, stock_decels_list, [0.1, 0.08, 0.06, 0.04, 0.02, 0.0]))
|
||||
self.assertEqual(actual, expected)
|
||||
self.assertEqual(self.controller.comfort_band_upper, 0.0)
|
||||
|
||||
for accel in accels_list:
|
||||
self.CS.out.aEgo = accel
|
||||
self.controller.calculate_comfort_band(self.CC, self.CS)
|
||||
actual = self.controller.comfort_band_upper
|
||||
expected = float(np.interp(accel, stock_accels_list, stock_comfort_band_vals))
|
||||
self.assertEqual(actual, expected)
|
||||
self.assertEqual(self.controller.comfort_band_lower, 0.0)
|
||||
|
||||
def test_update(self):
|
||||
self.CC.actuators.accel = 2.0
|
||||
self.CC.actuators.longControlState = LongCtrlState.pid
|
||||
self.CS.aBasis = 1.75
|
||||
self.CS.out.aEgo = 2.0
|
||||
self.CS.out.vEgo = 5.0
|
||||
|
||||
self.controller.update(self.CC, self.CS)
|
||||
self.assertEqual(self.controller.jerk_lower, 0.5)
|
||||
self.assertEqual(self.controller.jerk_upper, 3.0)
|
||||
self.assertEqual(self.controller.comfort_band_lower, 0.0)
|
||||
self.assertEqual(self.controller.comfort_band_upper, 0.10)
|
||||
self.assertEqual(self.controller.desired_accel, 2.0)
|
||||
@@ -0,0 +1,33 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from enum import IntFlag
|
||||
|
||||
|
||||
class HyundaiSafetyFlagsSP:
|
||||
DEFAULT = 0
|
||||
ESCC = 1
|
||||
LONG_MAIN_CRUISE_TOGGLEABLE = 2
|
||||
HAS_LDA_BUTTON = 4
|
||||
NON_SCC = 8
|
||||
|
||||
|
||||
class HyundaiFlagsSP(IntFlag):
|
||||
"""
|
||||
Flags for Hyundai specific quirks within sunnypilot.
|
||||
"""
|
||||
ENHANCED_SCC = 1
|
||||
HAS_LFA_BUTTON = 2 # Deprecated in favor of HyundaiFlags.HAS_LDA_BUTTON
|
||||
LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE = 2 ** 2
|
||||
ENABLE_RADAR_TRACKS_DEPRECATED = 2 ** 3
|
||||
LONG_TUNING_DYNAMIC = 2 ** 4
|
||||
LONG_TUNING_PREDICTIVE = 2 ** 5
|
||||
NON_SCC = 2 ** 6
|
||||
NON_SCC_RADAR_FCA = 2 ** 7 # most with FCA come from the camera
|
||||
NON_SCC_NO_FCA = 2 ** 8 # not all have FCA
|
||||
SPEED_LIMIT_AVAILABLE = 2 ** 9 # platforms with speed limit data available
|
||||
HAS_LKAS12 = 2 ** 10
|
||||
+18
@@ -0,0 +1,18 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from opendbc.car import structs
|
||||
|
||||
|
||||
class IntelligentCruiseButtonManagementInterfaceBase:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
self.CC_SP = None
|
||||
self.ICBM = None
|
||||
self.frame = 0
|
||||
self.button_frame = 0
|
||||
self.last_button_frame = 0
|
||||
@@ -0,0 +1,148 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import json
|
||||
import numpy as np
|
||||
from typing import NamedTuple
|
||||
from collections.abc import Callable
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.can_definitions import CanRecvCallable, CanSendCallable
|
||||
from opendbc.car.hyundai.values import HyundaiFlags
|
||||
from opendbc.car.subaru.values import SubaruFlags
|
||||
from opendbc.car.toyota.values import ToyotaSafetyFlags
|
||||
from opendbc.sunnypilot.car.hyundai.enable_radar_tracks import enable_radar_tracks as hyundai_enable_radar_tracks
|
||||
from opendbc.sunnypilot.car.hyundai.longitudinal.helpers import LongitudinalTuningType
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
from opendbc.sunnypilot.car.subaru.values_ext import SubaruFlagsSP, SubaruSafetyFlagsSP
|
||||
from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
|
||||
from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
|
||||
|
||||
|
||||
class LatControlInputs(NamedTuple):
|
||||
lateral_acceleration: float
|
||||
roll_compensation: float
|
||||
vego: float
|
||||
aego: float
|
||||
|
||||
|
||||
TorqueFromLateralAccelCallbackTypeTorqueSpace = Callable[[LatControlInputs, structs.CarParams.LateralTorqueTuning, bool], float]
|
||||
|
||||
|
||||
class CarInterfaceBaseSP:
|
||||
@staticmethod
|
||||
def torque_from_lateral_accel_linear_in_torque_space(latcontrol_inputs: LatControlInputs, torque_params: structs.CarParams.LateralTorqueTuning,
|
||||
gravity_adjusted: bool) -> float:
|
||||
# The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction)
|
||||
return latcontrol_inputs.lateral_acceleration / float(torque_params.latAccelFactor)
|
||||
|
||||
def torque_from_lateral_accel_in_torque_space(self) -> TorqueFromLateralAccelCallbackTypeTorqueSpace:
|
||||
return self.torque_from_lateral_accel_linear_in_torque_space
|
||||
|
||||
|
||||
class NanoFFModel:
|
||||
def __init__(self, weights_loc: str, platform: str):
|
||||
self.weights_loc = weights_loc
|
||||
self.platform = platform
|
||||
self.load_weights(platform)
|
||||
|
||||
def load_weights(self, platform: str):
|
||||
with open(self.weights_loc) as fob:
|
||||
self.weights = {k: np.array(v) for k, v in json.load(fob)[platform].items()}
|
||||
|
||||
def relu(self, x: np.ndarray):
|
||||
return np.maximum(0.0, x)
|
||||
|
||||
def forward(self, x: np.ndarray):
|
||||
assert x.ndim == 1
|
||||
x = (x - self.weights['input_norm_mat'][:, 0]) / (self.weights['input_norm_mat'][:, 1] - self.weights['input_norm_mat'][:, 0])
|
||||
x = self.relu(np.dot(x, self.weights['w_1']) + self.weights['b_1'])
|
||||
x = self.relu(np.dot(x, self.weights['w_2']) + self.weights['b_2'])
|
||||
x = self.relu(np.dot(x, self.weights['w_3']) + self.weights['b_3'])
|
||||
x = np.dot(x, self.weights['w_4']) + self.weights['b_4']
|
||||
return x
|
||||
|
||||
def predict(self, x: list[float], do_sample: bool = False):
|
||||
x = self.forward(np.array(x))
|
||||
if do_sample:
|
||||
pred = np.random.laplace(x[0], np.exp(x[1]) / self.weights['temperature'])
|
||||
else:
|
||||
pred = x[0]
|
||||
pred = pred * (self.weights['output_norm_mat'][1] - self.weights['output_norm_mat'][0]) + self.weights['output_norm_mat'][0]
|
||||
return pred
|
||||
|
||||
|
||||
def setup_interfaces(CI, CP: structs.CarParams, CP_SP: structs.CarParamsSP,
|
||||
params_list: list[dict[str, str]] | None = None,
|
||||
can_recv: CanRecvCallable | None = None, can_send: CanSendCallable | None = None) -> None:
|
||||
if params_list is None:
|
||||
params_list = []
|
||||
|
||||
params_dict = {k: v for param in params_list for k, v in param.items()}
|
||||
|
||||
_initialize_custom_longitudinal_tuning(CI, CP, CP_SP, params_dict)
|
||||
_initialize_coop_steering(CP, CP_SP, params_dict)
|
||||
_initialize_radar_tracks(CP, CP_SP, can_recv, can_send)
|
||||
_initialize_stop_and_go(CP, CP_SP, params_dict)
|
||||
_initialize_toyota(CP, CP_SP, params_dict)
|
||||
|
||||
|
||||
def _initialize_custom_longitudinal_tuning(CI, CP: structs.CarParams, CP_SP: structs.CarParamsSP,
|
||||
params_dict: dict[str, str]) -> None:
|
||||
|
||||
# Hyundai Custom Longitudinal Tuning
|
||||
if CP.brand == 'hyundai':
|
||||
hyundai_longitudinal_tuning = int(params_dict.get("HyundaiLongitudinalTuning", 0))
|
||||
if hyundai_longitudinal_tuning == LongitudinalTuningType.DYNAMIC:
|
||||
CP_SP.flags |= HyundaiFlagsSP.LONG_TUNING_DYNAMIC.value
|
||||
if hyundai_longitudinal_tuning == LongitudinalTuningType.PREDICTIVE:
|
||||
CP_SP.flags |= HyundaiFlagsSP.LONG_TUNING_PREDICTIVE.value
|
||||
|
||||
_ = CI.get_longitudinal_tuning_sp(CP, CP_SP)
|
||||
|
||||
|
||||
def _initialize_coop_steering(CP: structs.CarParams, CP_SP: structs.CarParamsSP,
|
||||
params_dict: dict[str, str]) -> None:
|
||||
if CP.brand == 'tesla':
|
||||
coop_steering = int(params_dict.get("TeslaCoopSteering", 0)) == 1
|
||||
if coop_steering:
|
||||
CP_SP.flags |= TeslaFlagsSP.COOP_STEERING.value
|
||||
|
||||
|
||||
def _initialize_radar_tracks(CP: structs.CarParams, CP_SP: structs.CarParamsSP,
|
||||
can_recv: CanRecvCallable | None = None, can_send: CanSendCallable | None = None) -> None:
|
||||
if CP.brand == 'hyundai':
|
||||
if CP.flags & HyundaiFlags.MANDO_RADAR and (CP.radarUnavailable or CP_SP.flags & HyundaiFlagsSP.ENHANCED_SCC):
|
||||
tracks_enabled = hyundai_enable_radar_tracks(can_recv, can_send, bus=0, addr=0x7d0)
|
||||
CP.radarUnavailable = not tracks_enabled
|
||||
|
||||
|
||||
def _initialize_stop_and_go(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params_dict: dict[str, str]) -> None:
|
||||
if CP.brand == 'subaru' and not CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID):
|
||||
stop_and_go = int(params_dict.get("SubaruStopAndGo", 0)) == 1
|
||||
stop_and_go_manual_parking_brake = int(params_dict.get("SubaruStopAndGoManualParkingBrake", 0)) == 1
|
||||
|
||||
if stop_and_go:
|
||||
CP_SP.flags |= SubaruFlagsSP.STOP_AND_GO.value
|
||||
if stop_and_go_manual_parking_brake:
|
||||
CP_SP.flags |= SubaruFlagsSP.STOP_AND_GO_MANUAL_PARKING_BRAKE.value
|
||||
if stop_and_go or stop_and_go_manual_parking_brake:
|
||||
CP_SP.safetyParam |= SubaruSafetyFlagsSP.STOP_AND_GO
|
||||
|
||||
|
||||
def _initialize_toyota(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params_dict: dict[str, str]) -> None:
|
||||
if CP.brand == 'toyota':
|
||||
toyota_stock_long = int(params_dict.get("ToyotaEnforceStockLongitudinal", 0)) == 1
|
||||
toyota_stop_and_go_hack = int(params_dict.get("ToyotaStopAndGoHack", 0)) == 1
|
||||
|
||||
if toyota_stock_long:
|
||||
CP_SP.flags |= ToyotaFlagsSP.STOCK_LONGITUDINAL.value
|
||||
CP.alphaLongitudinalAvailable = False
|
||||
CP.openpilotLongitudinalControl = False
|
||||
CP.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.STOCK_LONGITUDINAL.value
|
||||
|
||||
if toyota_stop_and_go_hack and CP.openpilotLongitudinalControl:
|
||||
CP_SP.flags |= ToyotaFlagsSP.STOP_AND_GO_HACK.value
|
||||
@@ -0,0 +1,21 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.lateral import apply_center_deadzone
|
||||
|
||||
|
||||
def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, friction_threshold: float,
|
||||
torque_params: structs.CarParams.LateralTorqueTuning) -> float:
|
||||
# TODO torque params' friction should be in lataxel space, not torque space
|
||||
friction_interp = np.interp(
|
||||
apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone),
|
||||
[-friction_threshold, friction_threshold],
|
||||
[-torque_params.friction, torque_params.friction]
|
||||
)
|
||||
return float(friction_interp)
|
||||
Whitespace-only changes.
@@ -0,0 +1,44 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from opendbc.car import structs, DT_CTRL
|
||||
from opendbc.car.can_definitions import CanData
|
||||
from opendbc.car.mazda import mazdacan
|
||||
from opendbc.car.mazda.values import Buttons
|
||||
from opendbc.sunnypilot.car.intelligent_cruise_button_management_interface_base import IntelligentCruiseButtonManagementInterfaceBase
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
SendButtonState = structs.IntelligentCruiseButtonManagement.SendButtonState
|
||||
|
||||
BUTTONS = {
|
||||
SendButtonState.increase: Buttons.SET_PLUS,
|
||||
SendButtonState.decrease: Buttons.SET_MINUS,
|
||||
}
|
||||
|
||||
|
||||
class IntelligentCruiseButtonManagementInterface(IntelligentCruiseButtonManagementInterfaceBase):
|
||||
def __init__(self, CP, CP_SP):
|
||||
super().__init__(CP, CP_SP)
|
||||
|
||||
def update(self, CC_SP, CS, packer, frame, last_button_frame) -> list[CanData]:
|
||||
can_sends = []
|
||||
self.CC_SP = CC_SP
|
||||
self.ICBM = CC_SP.intelligentCruiseButtonManagement
|
||||
self.frame = frame
|
||||
self.last_button_frame = last_button_frame
|
||||
|
||||
if self.ICBM.sendButton != SendButtonState.none:
|
||||
send_button = BUTTONS[self.ICBM.sendButton]
|
||||
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL > 0.2:
|
||||
self.button_frame += 1
|
||||
button_counter_offset = [1, 1, 0, None][self.button_frame % 4]
|
||||
if button_counter_offset is not None:
|
||||
can_sends.append(mazdacan.create_button_cmd(packer, self.CP, CS.crz_btns_counter + button_counter_offset, send_button))
|
||||
self.last_button_frame = self.frame
|
||||
|
||||
return can_sends
|
||||
Whitespace-only changes.
@@ -0,0 +1,34 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from enum import StrEnum
|
||||
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.sunnypilot.car.nissan.values import BUTTONS
|
||||
|
||||
|
||||
class CarStateExt:
|
||||
def __init__(self, CP, CP_SP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
self.button_events = []
|
||||
self.button_states = {button.event_type: False for button in BUTTONS}
|
||||
|
||||
def update(self, ret: structs.CarState, ret_sp: structs.CarStateSP, can_parsers: dict[StrEnum, CANParser]):
|
||||
cp = can_parsers[Bus.pt]
|
||||
|
||||
button_events = []
|
||||
for button in BUTTONS:
|
||||
state = (cp.vl[button.can_addr][button.can_msg] in button.values)
|
||||
if self.button_states[button.event_type] != state:
|
||||
event = structs.CarState.ButtonEvent.new_message()
|
||||
event.type = button.event_type
|
||||
event.pressed = state
|
||||
button_events.append(event)
|
||||
self.button_states[button.event_type] = state
|
||||
self.button_events = button_events
|
||||
@@ -0,0 +1,67 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from opendbc.car.nissan.values import CAR
|
||||
|
||||
LEAF_CRUISE_THROTTLE_SIGNALS = [
|
||||
"GAS_PEDAL",
|
||||
"GAS_PEDAL_INVERTED",
|
||||
"unsure2",
|
||||
"CRUISE_AVAILABLE",
|
||||
"unsure1",
|
||||
"PROPILOT_BUTTON",
|
||||
"CANCEL_BUTTON",
|
||||
"FOLLOW_DISTANCE_BUTTON",
|
||||
"SET_BUTTON",
|
||||
"RES_BUTTON",
|
||||
"NO_BUTTON_PRESSED",
|
||||
"unsure3",
|
||||
"COUNTER",
|
||||
"USER_BRAKE_PRESSED",
|
||||
"unsure5",
|
||||
"unsure6",
|
||||
"unsure7",
|
||||
]
|
||||
|
||||
XTRAIL_CRUISE_THROTTLE_SIGNALS = [
|
||||
"COUNTER",
|
||||
"PROPILOT_BUTTON",
|
||||
"CANCEL_BUTTON",
|
||||
"GAS_PEDAL_INVERTED",
|
||||
"SET_BUTTON",
|
||||
"RES_BUTTON",
|
||||
"FOLLOW_DISTANCE_BUTTON",
|
||||
"NO_BUTTON_PRESSED",
|
||||
"GAS_PEDAL",
|
||||
"USER_BRAKE_PRESSED",
|
||||
"USER_BRAKE_PRESSED_INVERTED",
|
||||
"NEW_SIGNAL_2",
|
||||
"GAS_PRESSED_INVERTED",
|
||||
"unsure1",
|
||||
"unsure2",
|
||||
"unsure3",
|
||||
]
|
||||
|
||||
|
||||
def create_cruise_throttle_msg(packer, car_fingerprint, cruise_throttle_msg, frame, button_name=None):
|
||||
signals = LEAF_CRUISE_THROTTLE_SIGNALS if car_fingerprint in (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC) else XTRAIL_CRUISE_THROTTLE_SIGNALS
|
||||
values = {s: cruise_throttle_msg[s] for s in signals}
|
||||
|
||||
can_bus = 1 if car_fingerprint == CAR.NISSAN_ALTIMA else 2
|
||||
|
||||
values["COUNTER"] = (frame // 2) % 4
|
||||
|
||||
if (button_name is not None) and (not values["CANCEL_BUTTON"]):
|
||||
values["NO_BUTTON_PRESSED"] = 0
|
||||
values["PROPILOT_BUTTON"] = 0
|
||||
values["SET_BUTTON"] = 0
|
||||
values["RES_BUTTON"] = 0
|
||||
values["FOLLOW_DISTANCE_BUTTON"] = 0
|
||||
values["CANCEL_BUTTON"] = 0
|
||||
values[button_name] = 1
|
||||
|
||||
return packer.make_can_msg("CRUISE_THROTTLE", can_bus, values)
|
||||
@@ -0,0 +1,24 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from collections import namedtuple
|
||||
from opendbc.car import structs
|
||||
|
||||
|
||||
class NissanSafetyFlagsSP:
|
||||
DEFAULT = 0
|
||||
LEAF = 1
|
||||
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
|
||||
|
||||
|
||||
BUTTONS = [
|
||||
Button(ButtonType.accelCruise, "CRUISE_THROTTLE", "RES_BUTTON", [1]),
|
||||
Button(ButtonType.decelCruise, "CRUISE_THROTTLE", "SET_BUTTON", [1]),
|
||||
]
|
||||
@@ -0,0 +1,84 @@
|
||||
import re
|
||||
import json
|
||||
import os
|
||||
import unicodedata
|
||||
|
||||
from opendbc.car import gen_empty_fingerprint
|
||||
from opendbc.car.common.basedir import BASEDIR
|
||||
from opendbc.car.docs import get_all_footnotes
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.mock.values import CAR as MOCK
|
||||
from opendbc.car.values import PLATFORMS
|
||||
|
||||
CAR_LIST_JSON_OUT = os.path.join(BASEDIR, "../", "sunnypilot", "car", "car_list.json")
|
||||
|
||||
|
||||
def _get_params_for_docs_sp(platform) -> tuple[CarParams, object]:
|
||||
cp_platform = platform if platform in interfaces else MOCK.MOCK
|
||||
CP: CarParams = interfaces[cp_platform].get_params(cp_platform, fingerprint=gen_empty_fingerprint(),
|
||||
car_fw=[CarParams.CarFw(ecu=CarParams.Ecu.unknown)],
|
||||
alpha_long=True, is_release=False, docs=True)
|
||||
CP_SP = interfaces[cp_platform].get_params_sp(CP, cp_platform, fingerprint=gen_empty_fingerprint(),
|
||||
car_fw=[CarParams.CarFw(ecu=CarParams.Ecu.unknown)],
|
||||
alpha_long=True, is_release_sp=True, docs=True)
|
||||
return CP, CP_SP
|
||||
|
||||
|
||||
def get_car_list() -> dict[str, dict[str, list[str] | str]]:
|
||||
collected_footnote = get_all_footnotes()
|
||||
sorted_list: dict[str, dict[str, list[str] | str]] = build_sorted_car_list(PLATFORMS, collected_footnote)
|
||||
return sorted_list
|
||||
|
||||
|
||||
def _natural_sort_key(s):
|
||||
# NFKD normalization ensures accented characters sort with their base letter (e.g., Š sorts with S)
|
||||
normalized = unicodedata.normalize('NFKD', s)
|
||||
return [int(t) if t.isdigit() else t.lower() for t in re.split(r'(\d+)', normalized) if t]
|
||||
|
||||
|
||||
def build_sorted_car_list(platforms, footnotes) -> dict[str, dict[str, list[str] | str]]:
|
||||
cars: dict[str, dict[str, list[str] | str]] = {}
|
||||
for model, platform in platforms.items():
|
||||
car_docs = platform.config.get_all_docs()
|
||||
CP, CP_SP = _get_params_for_docs_sp(platform)
|
||||
|
||||
if CP.dashcamOnly or not len(car_docs):
|
||||
continue
|
||||
|
||||
# A platform can include multiple car models
|
||||
for _car_docs in car_docs:
|
||||
if not hasattr(_car_docs, "row"):
|
||||
_car_docs.init_make(CP)
|
||||
_car_docs.init(CP, footnotes)
|
||||
cars[_car_docs.name] = model
|
||||
|
||||
_platform = model
|
||||
_name = _car_docs.name
|
||||
_make = _car_docs.make
|
||||
_brand = _car_docs.brand
|
||||
_model = _car_docs.model
|
||||
_years = _car_docs.year_list
|
||||
_package = _car_docs.package if _car_docs.package else []
|
||||
|
||||
cars[_name] = {
|
||||
"platform": _platform,
|
||||
"make": _make,
|
||||
"brand": _brand,
|
||||
"model": _model,
|
||||
"year": _years if _years else [],
|
||||
"package": _package,
|
||||
}
|
||||
|
||||
# Sort cars by make and model + year
|
||||
sorted_cars = sorted(cars.keys(), key=lambda car: _natural_sort_key(car))
|
||||
sorted_car_list = {car: cars[car] for car in sorted_cars}
|
||||
return sorted_car_list
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
platform_list = get_car_list()
|
||||
|
||||
with open(CAR_LIST_JSON_OUT, "w") as json_file:
|
||||
json.dump(platform_list, json_file, indent=2, ensure_ascii=False)
|
||||
print(f"Generated and written to {CAR_LIST_JSON_OUT}")
|
||||
Whitespace-only changes.
@@ -0,0 +1,103 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import math
|
||||
from enum import StrEnum
|
||||
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.rivian.values import DBC
|
||||
from opendbc.sunnypilot.car.rivian.values import RivianFlagsSP
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
|
||||
MAX_SET_SPEED = 85 * CV.MPH_TO_MS
|
||||
MIN_SET_SPEED = 20 * CV.MPH_TO_MS
|
||||
|
||||
|
||||
class CarStateExt:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
self.set_speed = 10
|
||||
self.increase_button = False
|
||||
self.decrease_button = False
|
||||
self.distance_button = 0
|
||||
self.increase_counter = 0
|
||||
self.decrease_counter = 0
|
||||
self.stalk_down_counter = 0
|
||||
|
||||
def update_longitudinal_upgrade(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
cp_park = can_parsers[Bus.alt]
|
||||
cp_adas = can_parsers[Bus.adas]
|
||||
cp = can_parsers[Bus.pt]
|
||||
|
||||
prev_increase_button = self.increase_button
|
||||
prev_decrease_button = self.decrease_button
|
||||
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
# distance scroll wheel
|
||||
right_scroll = cp_park.vl["WheelButtons_Fwd"]["RightButton_Scroll"]
|
||||
if right_scroll != 255:
|
||||
if self.distance_button != right_scroll:
|
||||
ret.buttonEvents = [structs.CarState.ButtonEvent(pressed=False, type=ButtonType.gapAdjustCruise)]
|
||||
self.distance_button = right_scroll
|
||||
|
||||
# button logic for set-speed
|
||||
self.increase_button = cp_park.vl["WheelButtons_Fwd"]["RightButton_RightClick"] == 2
|
||||
self.decrease_button = cp_park.vl["WheelButtons_Fwd"]["RightButton_LeftClick"] == 2
|
||||
|
||||
self.increase_counter = self.increase_counter + 1 if self.increase_button else 0
|
||||
self.decrease_counter = self.decrease_counter + 1 if self.decrease_button else 0
|
||||
|
||||
metric = cp_adas.vl["Cluster"]["Cluster_Unit"] == 0
|
||||
conversion = CV.KPH_TO_MS if metric else CV.MPH_TO_MS
|
||||
long_press_step = 10.0 if metric else 5.0
|
||||
set_speed_converted = self.set_speed * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH)
|
||||
|
||||
if self.increase_button:
|
||||
if self.increase_counter % 66 == 0:
|
||||
self.set_speed = (int(math.ceil((set_speed_converted + 1) / long_press_step)) * long_press_step) * conversion
|
||||
elif not prev_increase_button:
|
||||
self.set_speed += conversion
|
||||
|
||||
if self.decrease_button:
|
||||
if self.decrease_counter % 66 == 0:
|
||||
self.set_speed = (int(math.floor((set_speed_converted - 1) / long_press_step)) * long_press_step) * conversion
|
||||
elif not prev_decrease_button:
|
||||
self.set_speed -= conversion
|
||||
|
||||
if not ret.cruiseState.enabled:
|
||||
self.set_speed = ret.vEgoCluster
|
||||
|
||||
# VDM_UserAdasRequest: 0=IDLE, 1=UP_1, 2=UP_2, 3=DOWN_1, 4=DOWN_2
|
||||
stalk_down = int(cp.vl["VDM_AdasSts"]["VDM_UserAdasRequest"]) in (3, 4)
|
||||
self.stalk_down_counter = self.stalk_down_counter + 1 if stalk_down else 0
|
||||
if self.stalk_down_counter == 50:
|
||||
# Mimic Rivian ACC: holding stalk 0.5s sets speed to current speed (never decreases)
|
||||
self.set_speed = max(self.set_speed, ret.vEgoCluster)
|
||||
|
||||
self.set_speed = max(MIN_SET_SPEED, min(self.set_speed, MAX_SET_SPEED))
|
||||
ret.cruiseState.speed = self.set_speed
|
||||
|
||||
if self.CP.enableBsm:
|
||||
ret.leftBlindspot = cp_park.vl["BSM_BlindSpotIndicator_Fwd"]["BSM_BlindSpotIndicator_Left"] != 0
|
||||
ret.rightBlindspot = cp_park.vl["BSM_BlindSpotIndicator_Fwd"]["BSM_BlindSpotIndicator_Right"] != 0
|
||||
|
||||
def update(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
if self.CP_SP.flags & RivianFlagsSP.LONGITUDINAL_HARNESS_UPGRADE:
|
||||
self.update_longitudinal_upgrade(ret, can_parsers)
|
||||
|
||||
@staticmethod
|
||||
def get_parser(CP, CP_SP) -> dict[StrEnum, CANParser]:
|
||||
messages = {}
|
||||
|
||||
if CP_SP.flags & RivianFlagsSP.LONGITUDINAL_HARNESS_UPGRADE:
|
||||
messages[Bus.alt] = CANParser(DBC[CP.carFingerprint][Bus.alt], [], 1)
|
||||
|
||||
return messages
|
||||
@@ -0,0 +1,36 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from collections import namedtuple
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
|
||||
MAX_STEERING_ANGLE = 90.0
|
||||
|
||||
MadsDataSP = namedtuple("MadsDataSP",
|
||||
["lka_icon_states", "lat_active"])
|
||||
|
||||
|
||||
class MadsCarController:
|
||||
def __init__(self):
|
||||
self.mads = MadsDataSP(False, False)
|
||||
|
||||
self.lka_icon_states = False
|
||||
self.lat_active = False
|
||||
|
||||
def mads_status_update(self, CC: structs.CarControl, CC_SP: structs.CarControlSP, CS: CarStateBase) -> MadsDataSP:
|
||||
if CC_SP.mads.available:
|
||||
self.lka_icon_states = self.lat_active
|
||||
self.lat_active = CC.latActive and abs(CS.out.steeringAngleDeg) < MAX_STEERING_ANGLE
|
||||
else:
|
||||
self.lka_icon_states = CC.enabled
|
||||
self.lat_active = CC.latActive
|
||||
|
||||
return MadsDataSP(self.lka_icon_states, self.lat_active)
|
||||
|
||||
def update(self, CC: structs.CarControl, CC_SP: structs.CarControlSP, CS: CarStateBase) -> None:
|
||||
self.mads = self.mads_status_update(CC, CC_SP, CS)
|
||||
@@ -0,0 +1,11 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from enum import IntFlag
|
||||
|
||||
|
||||
class RivianFlagsSP(IntFlag):
|
||||
LONGITUDINAL_HARNESS_UPGRADE = 1
|
||||
Whitespace-only changes.
@@ -0,0 +1,48 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from enum import StrEnum
|
||||
from opendbc.car import Bus, structs
|
||||
|
||||
from opendbc.car.subaru.values import SubaruFlags
|
||||
from opendbc.sunnypilot.mads_base import MadsCarStateBase
|
||||
from opendbc.can.parser import CANParser
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class MadsCarState(MadsCarStateBase):
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
super().__init__(CP, CP_SP)
|
||||
|
||||
@staticmethod
|
||||
def create_lkas_button_events(cur_btn: int, prev_btn: int,
|
||||
buttons_dict: dict[int, structs.CarState.ButtonEvent.Type]) -> list[structs.CarState.ButtonEvent]:
|
||||
events: list[structs.CarState.ButtonEvent] = []
|
||||
|
||||
if cur_btn == prev_btn:
|
||||
return events
|
||||
|
||||
state_changes = [
|
||||
{"pressed": prev_btn != cur_btn and cur_btn != 2 and not (prev_btn == 2 and cur_btn == 1)},
|
||||
{"pressed": prev_btn != cur_btn and cur_btn == 2 and cur_btn != 1},
|
||||
]
|
||||
|
||||
for change in state_changes:
|
||||
if change["pressed"]:
|
||||
events.append(structs.CarState.ButtonEvent(pressed=change["pressed"],
|
||||
type=buttons_dict.get(cur_btn, ButtonType.unknown)))
|
||||
return events
|
||||
|
||||
def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
|
||||
self.prev_lkas_button = self.lkas_button
|
||||
if not self.CP.flags & SubaruFlags.PREGLOBAL:
|
||||
self.lkas_button = cp_cam.vl["ES_LKAS_State"]["LKAS_Dash_State"]
|
||||
|
||||
ret.buttonEvents = self.create_lkas_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas})
|
||||
@@ -0,0 +1,124 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import copy
|
||||
from enum import StrEnum
|
||||
|
||||
from opendbc.car import Bus, structs, DT_CTRL
|
||||
from opendbc.car.can_definitions import CanData
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.subaru.values import SubaruFlags
|
||||
|
||||
from opendbc.sunnypilot.car.subaru import subarucan_ext
|
||||
from opendbc.sunnypilot.car.subaru.values_ext import SubaruFlagsSP
|
||||
from opendbc.can.parser import CANParser
|
||||
|
||||
_SNG_ACC_MIN_DIST = 3
|
||||
_SNG_ACC_MAX_DIST = 4.5
|
||||
|
||||
|
||||
class SnGCarController:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
self.enabled = CP_SP.flags & (SubaruFlagsSP.STOP_AND_GO | SubaruFlagsSP.STOP_AND_GO_MANUAL_PARKING_BRAKE)
|
||||
self.manual_parking_brake = CP_SP.flags & SubaruFlagsSP.STOP_AND_GO_MANUAL_PARKING_BRAKE
|
||||
|
||||
self.last_standstill_frame = 0
|
||||
self.epb_resume_frames_remaining = -1
|
||||
self.prev_close_distance = 0.0
|
||||
|
||||
def update_epb_resume_sequence(self, should_resume: bool) -> bool:
|
||||
if self.manual_parking_brake:
|
||||
return False
|
||||
|
||||
if should_resume:
|
||||
self.epb_resume_frames_remaining = 15
|
||||
|
||||
send_resume = self.epb_resume_frames_remaining > 0
|
||||
if self.epb_resume_frames_remaining > 0:
|
||||
self.epb_resume_frames_remaining -= 1
|
||||
|
||||
return send_resume
|
||||
|
||||
def update_stop_and_go(self, CC: structs.CarControl, CS: CarStateBase, frame: int) -> bool:
|
||||
"""
|
||||
Manages stop-and-go functionality for adaptive cruise control (ACC).
|
||||
|
||||
Args:
|
||||
CC: Car control data
|
||||
CS: Car state data
|
||||
frame: Current frame number
|
||||
|
||||
Returns:
|
||||
bool: True if resume command should be sent, False otherwise
|
||||
"""
|
||||
|
||||
if not CC.enabled or not CC.hudControl.leadVisible:
|
||||
return False
|
||||
|
||||
close_distance = CS.es_distance_msg["Close_Distance"]
|
||||
in_standstill = CS.out.standstill
|
||||
|
||||
if not in_standstill:
|
||||
self.last_standstill_frame = frame
|
||||
|
||||
# Check if we've been in standstill long enough
|
||||
mpb_standstill_timers = (0.75, 0.8) if self.CP.flags & SubaruFlags.PREGLOBAL else (0.5, 0.55)
|
||||
standstill_duration = (frame - self.last_standstill_frame) * DT_CTRL
|
||||
in_standstill_hold = standstill_duration > mpb_standstill_timers[0]
|
||||
if (frame - self.last_standstill_frame) * DT_CTRL >= mpb_standstill_timers[1]:
|
||||
self.last_standstill_frame = frame
|
||||
|
||||
# Car state distance-based conditions (EPB only)
|
||||
in_resume_distance = _SNG_ACC_MIN_DIST < close_distance < _SNG_ACC_MAX_DIST
|
||||
distance_increasing = close_distance > self.prev_close_distance
|
||||
distance_resume_allowed = in_resume_distance and distance_increasing
|
||||
|
||||
if self.manual_parking_brake:
|
||||
# Manual parking brake: Direct resume when the standstill hold threshold is reached to prevent ACC fault
|
||||
send_resume = in_standstill_hold
|
||||
else:
|
||||
# EPB: Resume sequence with trigger on distance with lead car increasing
|
||||
should_resume = CS.out.standstill and distance_resume_allowed
|
||||
send_resume = self.update_epb_resume_sequence(should_resume)
|
||||
|
||||
self.prev_close_distance = close_distance
|
||||
|
||||
return send_resume
|
||||
|
||||
def create_stop_and_go(self, packer, CC: structs.CarControl, CS: CarStateBase, frame: int) -> list[CanData]:
|
||||
can_sends = []
|
||||
|
||||
if not self.enabled:
|
||||
return can_sends
|
||||
|
||||
send_resume = self.update_stop_and_go(CC, CS, frame)
|
||||
|
||||
can_sends.append(subarucan_ext.create_throttle(packer, self.CP, CS.throttle_msg, send_resume and not self.manual_parking_brake))
|
||||
|
||||
if frame % 2 == 0:
|
||||
can_sends.append(subarucan_ext.create_brake_pedal(packer, self.CP, CS.brake_pedal_msg, send_resume and self.manual_parking_brake))
|
||||
|
||||
return can_sends
|
||||
|
||||
|
||||
class SnGCarState:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
self.brake_pedal_msg: dict[str, float] = {}
|
||||
self.throttle_msg: dict[str, float] = {}
|
||||
|
||||
def update(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
cp = can_parsers[Bus.pt]
|
||||
|
||||
self.brake_pedal_msg = copy.copy(cp.vl["Brake_Pedal"])
|
||||
|
||||
if not self.CP.flags & SubaruFlags.HYBRID:
|
||||
self.throttle_msg = copy.copy(cp.vl["Throttle"])
|
||||
@@ -0,0 +1,68 @@
|
||||
from opendbc.car.subaru.values import CanBus, SubaruFlags
|
||||
|
||||
|
||||
def create_counter(msg):
|
||||
return (msg["COUNTER"] + 1) % 0x10
|
||||
|
||||
|
||||
def create_throttle(packer, CP, throttle_msg, send_resume):
|
||||
if CP.flags & SubaruFlags.PREGLOBAL:
|
||||
values = {s: throttle_msg[s] for s in [
|
||||
"Throttle_Pedal",
|
||||
"Signal1",
|
||||
"Not_Full_Throttle",
|
||||
"Signal2",
|
||||
"Engine_RPM",
|
||||
"Off_Throttle",
|
||||
"Signal3",
|
||||
"Throttle_Cruise",
|
||||
"Throttle_Combo",
|
||||
"Throttle_Body",
|
||||
"Off_Throttle_2",
|
||||
"Signal4",
|
||||
]}
|
||||
else:
|
||||
values = {s: throttle_msg[s] for s in [
|
||||
"CHECKSUM",
|
||||
"Signal1",
|
||||
"Engine_RPM",
|
||||
"Neutral",
|
||||
"Throttle_Pedal",
|
||||
"Throttle_Cruise",
|
||||
"Throttle_Combo",
|
||||
"Signal3",
|
||||
"Off_Accel",
|
||||
]}
|
||||
|
||||
values["COUNTER"] = create_counter(throttle_msg)
|
||||
|
||||
if send_resume:
|
||||
values["Throttle_Pedal"] = 5
|
||||
|
||||
return packer.make_can_msg("Throttle", CanBus.camera, values)
|
||||
|
||||
|
||||
def create_brake_pedal(packer, CP, brake_pedal_msg, send_resume):
|
||||
if CP.flags & SubaruFlags.PREGLOBAL:
|
||||
values = {s: brake_pedal_msg[s] for s in [
|
||||
"Speed",
|
||||
"Brake_Pedal",
|
||||
"Signal1",
|
||||
]}
|
||||
else:
|
||||
values = {s: brake_pedal_msg[s] for s in [
|
||||
"CHECKSUM",
|
||||
"Signal1",
|
||||
"Speed",
|
||||
"Signal2",
|
||||
"Brake_Lights",
|
||||
"Signal3",
|
||||
"Brake_Pedal",
|
||||
"Signal4",
|
||||
]}
|
||||
values["COUNTER"] = create_counter(brake_pedal_msg)
|
||||
|
||||
if send_resume:
|
||||
values["Speed"] = 1 if CP.flags & SubaruFlags.PREGLOBAL else 3
|
||||
|
||||
return packer.make_can_msg("Brake_Pedal", CanBus.camera, values)
|
||||
@@ -0,0 +1,17 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from enum import IntFlag
|
||||
|
||||
|
||||
class SubaruSafetyFlagsSP:
|
||||
STOP_AND_GO = 1
|
||||
|
||||
|
||||
class SubaruFlagsSP(IntFlag):
|
||||
STOP_AND_GO = 1
|
||||
STOP_AND_GO_MANUAL_PARKING_BRAKE = 2
|
||||
Whitespace-only changes.
@@ -0,0 +1,55 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from enum import StrEnum
|
||||
|
||||
from opendbc.car import Bus, create_button_events, structs
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.tesla.values import DBC, CANBUS
|
||||
from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class CarStateExt:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
self.infotainment_3_finger_press = 0
|
||||
|
||||
def update(self, ret: structs.CarState, ret_sp: structs.CarStateSP, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
if self.CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS:
|
||||
cp_adas = can_parsers[Bus.adas]
|
||||
|
||||
prev_infotainment_3_finger_press = self.infotainment_3_finger_press
|
||||
self.infotainment_3_finger_press = int(cp_adas.vl["UI_status2"]["UI_activeTouchPoints"])
|
||||
|
||||
ret.buttonEvents = [*create_button_events(self.infotainment_3_finger_press, prev_infotainment_3_finger_press,
|
||||
{3: ButtonType.lkas})]
|
||||
|
||||
cp_party = can_parsers[Bus.party]
|
||||
cp_ap_party = can_parsers[Bus.ap_party]
|
||||
|
||||
speed_units = self.can_define.dv["DI_state"]["DI_speedUnits"].get(int(cp_party.vl["DI_state"]["DI_speedUnits"]), None)
|
||||
speed_limit = cp_ap_party.vl["DAS_status"]["DAS_fusedSpeedLimit"]
|
||||
if self.can_define.dv["DAS_status"]["DAS_fusedSpeedLimit"].get(int(speed_limit), None) in ["NONE", "UNKNOWN_SNA"]:
|
||||
ret_sp.speedLimit = 0
|
||||
else:
|
||||
if speed_units == "KPH":
|
||||
ret_sp.speedLimit = speed_limit * CV.KPH_TO_MS
|
||||
elif speed_units == "MPH":
|
||||
ret_sp.speedLimit = speed_limit * CV.MPH_TO_MS
|
||||
|
||||
@staticmethod
|
||||
def get_parser(CP: structs.CarParams, CP_SP: structs.CarParamsSP) -> dict[StrEnum, CANParser]:
|
||||
messages = {}
|
||||
|
||||
if CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS:
|
||||
messages[Bus.adas] = CANParser(DBC[CP.carFingerprint][Bus.adas], [], CANBUS.vehicle)
|
||||
|
||||
return messages
|
||||
@@ -0,0 +1,28 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from collections import namedtuple
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
|
||||
|
||||
CoopSteeringDataSP = namedtuple("CoopSteeringDataSP",
|
||||
["control_type"])
|
||||
|
||||
|
||||
class CoopSteeringCarController:
|
||||
def __init__(self):
|
||||
self.coop_steering = CoopSteeringDataSP(False)
|
||||
|
||||
@staticmethod
|
||||
def coop_steering_status_update(CP_SP: structs.CarParamsSP) -> CoopSteeringDataSP:
|
||||
coop_steering = CP_SP.flags & TeslaFlagsSP.COOP_STEERING.value
|
||||
control_type = 2 if coop_steering else 1
|
||||
|
||||
return CoopSteeringDataSP(control_type)
|
||||
|
||||
def update(self, CP_SP: structs.CarParamsSP) -> None:
|
||||
self.coop_steering = self.coop_steering_status_update(CP_SP)
|
||||
@@ -0,0 +1,12 @@
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.tesla.values import CAR
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
FW_VERSIONS_EXT = {
|
||||
CAR.TESLA_MODEL_3: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'TeMYG4_Main_0.0.0 (67),E4HP015.02.1',
|
||||
],
|
||||
},
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from enum import IntFlag
|
||||
|
||||
|
||||
class TeslaFlagsSP(IntFlag):
|
||||
HAS_VEHICLE_BUS = 1 # 3-finger infotainment press signal is present on the VEHICLE bus with the deprecated Tesla harness installed
|
||||
COOP_STEERING = 2 # Coop steering
|
||||
|
||||
|
||||
class TeslaSafetyFlagsSP:
|
||||
HAS_VEHICLE_BUS = 1
|
||||
Whitespace-only changes.
@@ -0,0 +1,13 @@
|
||||
import json
|
||||
import unittest
|
||||
|
||||
from opendbc.sunnypilot.car.platform_list import get_car_list, CAR_LIST_JSON_OUT
|
||||
|
||||
|
||||
class TestCarList(unittest.TestCase):
|
||||
def test_generator(self):
|
||||
generated_car_list = json.dumps(get_car_list(), indent=2, ensure_ascii=False)
|
||||
with open(CAR_LIST_JSON_OUT) as f:
|
||||
current_car_list = f.read()
|
||||
|
||||
assert generated_car_list == current_car_list, "Run opendbc/sunnypilot/car/platform_list.py to update the car list"
|
||||
@@ -0,0 +1 @@
|
||||
{"CHEVROLET_BOLT_EUV": {"w_1": [[0.3452189564704895, -0.15614677965641022, -0.04062516987323761, -0.5960758328437805, 0.3211185932159424, 0.31732726097106934, -0.04430829733610153, -0.37327295541763306, -0.14118380844593048, 0.12712529301643372, 0.2641555070877075, -0.3451094627380371, -0.005127656273543835, 0.6185108423233032, 0.03725295141339302, 0.3763789236545563], [-0.0708412230014801, 0.3667356073856354, 0.031383827328681946, 0.1740853488445282, -0.04695861041545868, 0.018055908381938934, 0.009072160348296165, -0.23640218377113342, -0.10362917929887772, 0.022628149017691612, -0.224413201212883, 0.20718418061733246, -0.016947750002145767, -0.3872031271457672, -0.15500062704086304, -0.06375953555107117], [-0.0838046595454216, -0.0242826659232378, -0.07765661180019379, 0.028858814388513565, -0.09516210108995438, 0.008368706330657005, 0.1689300835132599, 0.015036891214549541, -0.15121428668498993, 0.1388195902109146, 0.11486363410949707, 0.0651545450091362, 0.13559958338737488, 0.04300367832183838, -0.13856294751167297, -0.058136988431215286], [-0.006249868310987949, 0.08809533715248108, -0.040690965950489044, 0.02359287068247795, -0.00766348373144865, 0.24816390872001648, -0.17360293865203857, -0.03676899895071983, -0.17564819753170013, 0.18998438119888306, -0.050583917647600174, -0.006488069426268339, 0.10649101436138153, -0.024557121098041534, -0.103276826441288, 0.18448011577129364]], "b_1": [0.2935388386249542, 0.10967712104320526, -0.014007942751049995, 0.211833655834198, 0.33605605363845825, 0.37722209095954895, -0.16615016758441925, 0.3134673535823822, 0.06695777177810669, 0.3425212800502777, 0.3769673705101013, 0.23186539113521576, 0.5770409107208252, -0.05929069593548775, 0.01839117519557476, 0.03828774020075798], "w_2": [[-0.06261160969734192, 0.010185074992477894, -0.06083013117313385, -0.04531499370932579, -0.08979734033346176, 0.3432150185108185, -0.019801849499344826, 0.3010321259498596], [0.19698476791381836, -0.009238275699317455, 0.08842222392559052, -0.09516377002000809, -0.05022778362035751, 0.13626104593276978, -0.052890390157699585, 0.15569131076335907], [0.0724768117070198, -0.09018408507108688, 0.06850195676088333, -0.025572121143341064, 0.0680626779794693, -0.07648195326328278, 0.07993496209383011, -0.059752143919467926], [1.267876386642456, -0.05755887180566788, -0.08429178595542908, 0.021366603672504425, -0.0006479775765910745, -1.4292563199996948, -0.08077696710824966, -1.414825439453125], [0.04535430669784546, 0.06555880606174469, -0.027145234867930412, -0.07661093026399612, -0.05702832341194153, 0.23650476336479187, 0.0024587824009358883, 0.20126521587371826], [0.006042032968252897, 0.042880818247795105, 0.002187949838116765, -0.017126334831118584, -0.08352015167474747, 0.19801731407642365, -0.029196614399552345, 0.23713473975658417], [-0.01644900068640709, -0.04358499124646187, 0.014584392309188843, 0.07155826687812805, -0.09354910999536514, -0.033351872116327286, 0.07138452678918839, -0.04755295440554619], [-1.1012420654296875, -0.03534531593322754, 0.02167935110628605, -0.01116552110761404, -0.08436500281095505, 1.1038788557052612, 0.027903547510504723, 1.0676132440567017], [0.03843916580080986, -0.0952216386795044, 0.039226632565259933, 0.002778085647150874, -0.020275786519050598, -0.07848760485649109, 0.04803166165947914, 0.015538203530013561], [0.018385495990514755, -0.025189843028783798, 0.0036680365446954966, -0.02105865254998207, 0.04808586835861206, 0.1575016975402832, 0.02703506126999855, 0.23039312660694122], [-0.0033881019335240126, -0.10210853815078735, -0.04877309128642082, 0.006989633198827505, 0.046798162162303925, 0.38676899671554565, -0.032304272055625916, 0.2345031052827835], [0.22092825174331665, -0.09642873704433441, 0.04499409720301628, 0.05108088254928589, -0.10191166400909424, 0.12818090617656708, -0.021021494641900063, 0.09440375864505768], [0.1212429478764534, -0.028194155544042587, -0.0981956496834755, 0.08226924389600754, 0.055346839129924774, 0.27067816257476807, -0.09064067900180817, 0.12580905854701996], [-1.6740131378173828, -0.02066155895590782, -0.05924689769744873, 0.06347910314798355, -0.07821853458881378, 1.2807466983795166, 0.04589352011680603, 1.310766577720642], [-0.09893272817134857, -0.04093599319458008, -0.02502273954451084, 0.09490344673395157, -0.0211324505507946, -0.09021010994911194, 0.07936318963766098, -0.03593116253614426], [-0.08490308374166489, -0.015558987855911255, -0.048692114651203156, -0.007421435788273811, -0.040531404316425323, 0.25889304280281067, 0.06012800335884094, 0.27946868538856506]], "b_2": [0.07973937690258026, -0.010446485131978989, -0.003066520905122161, -0.031895797699689865, 0.006032303906977177, 0.24106740951538086, -0.008969511836767197, 0.2872662842273712], "w_3": [[-1.364486813545227, -0.11682678014039993, 0.01764785870909691, 0.03926877677440643], [-0.05695437639951706, 0.05472218990325928, 0.1266128271818161, 0.09950875490903854], [0.11415273696184158, -0.10069356113672256, 0.0864749327301979, -0.Line truncated
|
||||
Whitespace-only changes.
@@ -0,0 +1,170 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from enum import StrEnum
|
||||
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.carlog import carlog
|
||||
from opendbc.can.parser import CANParser
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
|
||||
|
||||
TRAFFIC_SIGNAL_MAP = {
|
||||
1: "kph",
|
||||
36: "mph",
|
||||
65: "No overtake",
|
||||
66: "No overtake"
|
||||
}
|
||||
|
||||
ZSS_DIFF_THRESHOLD = 4
|
||||
ZSS_MAX_THRESHOLD = 10
|
||||
|
||||
|
||||
class CarStateExt:
|
||||
def __init__(self, CP, CP_SP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
self.acc_type = 1
|
||||
self.zss_compute = False
|
||||
self.zss_cruise_active_last = False
|
||||
self.zss_angle_offset = 0.
|
||||
self.zss_threshold_count = 0
|
||||
|
||||
"""Initialize traffic signal variables"""
|
||||
self._tsgn1 = None
|
||||
self._spdval1 = None
|
||||
self._splsgn1 = None
|
||||
self._tsgn2 = None
|
||||
self._splsgn2 = None
|
||||
self._tsgn3 = None
|
||||
self._splsgn3 = None
|
||||
self._tsgn4 = None
|
||||
self._splsgn4 = None
|
||||
|
||||
@staticmethod
|
||||
def traffic_signal_description(tsgn):
|
||||
"""Get description for traffic signal code"""
|
||||
desc = TRAFFIC_SIGNAL_MAP.get(int(tsgn))
|
||||
return f'{tsgn}: {desc}' if desc is not None else f'{tsgn}'
|
||||
|
||||
def update_traffic_signals(self, cp_cam):
|
||||
"""Update traffic signals with error handling"""
|
||||
try:
|
||||
# Add error handling for missing RSA messages
|
||||
tsgn1 = cp_cam.vl.get("RSA1", {}).get('TSGN1', 0)
|
||||
spdval1 = cp_cam.vl.get("RSA1", {}).get('SPDVAL1', 0)
|
||||
splsgn1 = cp_cam.vl.get("RSA1", {}).get('SPLSGN1', 0)
|
||||
tsgn2 = cp_cam.vl.get("RSA1", {}).get('TSGN2', 0)
|
||||
splsgn2 = cp_cam.vl.get("RSA1", {}).get('SPLSGN2', 0)
|
||||
tsgn3 = cp_cam.vl.get("RSA2", {}).get('TSGN3', 0)
|
||||
splsgn3 = cp_cam.vl.get("RSA2", {}).get('SPLSGN3', 0)
|
||||
tsgn4 = cp_cam.vl.get("RSA2", {}).get('TSGN4', 0)
|
||||
splsgn4 = cp_cam.vl.get("RSA2", {}).get('SPLSGN4', 0)
|
||||
except (KeyError, AttributeError) as e:
|
||||
# Handle case where RSA messages are not available
|
||||
carlog.debug(f"RSA messages not available: {e}")
|
||||
return
|
||||
|
||||
has_changed = tsgn1 != self._tsgn1 \
|
||||
or spdval1 != self._spdval1 \
|
||||
or splsgn1 != self._splsgn1 \
|
||||
or tsgn2 != self._tsgn2 \
|
||||
or splsgn2 != self._splsgn2 \
|
||||
or tsgn3 != self._tsgn3 \
|
||||
or splsgn3 != self._splsgn3 \
|
||||
or tsgn4 != self._tsgn4 \
|
||||
or splsgn4 != self._splsgn4
|
||||
|
||||
self._tsgn1 = tsgn1
|
||||
self._spdval1 = spdval1
|
||||
self._splsgn1 = splsgn1
|
||||
self._tsgn2 = tsgn2
|
||||
self._splsgn2 = splsgn2
|
||||
self._tsgn3 = tsgn3
|
||||
self._splsgn3 = splsgn3
|
||||
self._tsgn4 = tsgn4
|
||||
self._splsgn4 = splsgn4
|
||||
|
||||
if not has_changed:
|
||||
return
|
||||
|
||||
carlog.debug('---- TRAFFIC SIGNAL UPDATE -----')
|
||||
if tsgn1 is not None and tsgn1 != 0:
|
||||
carlog.debug(f'TSGN1: {self.traffic_signal_description(tsgn1)}')
|
||||
if spdval1 is not None and spdval1 != 0:
|
||||
carlog.debug(f'SPDVAL1: {spdval1}')
|
||||
if splsgn1 is not None and splsgn1 != 0:
|
||||
carlog.debug(f'SPLSGN1: {splsgn1}')
|
||||
if tsgn2 is not None and tsgn2 != 0:
|
||||
carlog.debug(f'TSGN2: {self.traffic_signal_description(tsgn2)}')
|
||||
if splsgn2 is not None and splsgn2 != 0:
|
||||
carlog.debug(f'SPLSGN2: {splsgn2}')
|
||||
if tsgn3 is not None and tsgn3 != 0:
|
||||
carlog.debug(f'TSGN3: {self.traffic_signal_description(tsgn3)}')
|
||||
if splsgn3 is not None and splsgn3 != 0:
|
||||
carlog.debug(f'SPLSGN3: {splsgn3}')
|
||||
if tsgn4 is not None and tsgn4 != 0:
|
||||
carlog.debug(f'TSGN4: {self.traffic_signal_description(tsgn4)}')
|
||||
if splsgn4 is not None and splsgn4 != 0:
|
||||
carlog.debug(f'SPLSGN4: {splsgn4}')
|
||||
carlog.debug('------------------------')
|
||||
|
||||
def calculate_speed_limit(self):
|
||||
"""Calculate speed limit from traffic signals with validation"""
|
||||
# Check all traffic sign slots for speed limits, not just tsgn1
|
||||
for tsgn, spdval in [(self._tsgn1, self._spdval1),
|
||||
(self._tsgn2, None),
|
||||
(self._tsgn3, None),
|
||||
(self._tsgn4, None)]:
|
||||
|
||||
if tsgn == 1 and spdval is not None and 0 < spdval <= 200: # Reasonable speed range
|
||||
return spdval * CV.KPH_TO_MS
|
||||
|
||||
if tsgn == 36 and spdval is not None and 0 < spdval <= 120: # Reasonable MPH range
|
||||
return spdval * CV.MPH_TO_MS
|
||||
|
||||
return 0
|
||||
|
||||
def update(self, ret: structs.CarState, ret_sp: structs.CarStateSP, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
|
||||
if self.CP_SP.flags & ToyotaFlagsSP.SMART_DSU:
|
||||
self.acc_type = 1
|
||||
|
||||
if self.CP_SP.enableGasInterceptor:
|
||||
gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
|
||||
ret.gasPressed = gas > 805
|
||||
|
||||
# ZSS support thanks to zorrobyte, ErichMoraga, and dragonpilot
|
||||
if self.CP_SP.flags & ToyotaFlagsSP.ZSS:
|
||||
zorro_steer = cp.vl["SECONDARY_STEER_ANGLE"]["ZORRO_STEER"]
|
||||
control_available = ret.cruiseState.available
|
||||
|
||||
# Only compute ZSS offset when control is available
|
||||
if control_available and not self.zss_cruise_active_last:
|
||||
self.zss_threshold_count = 0
|
||||
self.zss_compute = True # Control was just activated, so allow offset to be recomputed
|
||||
self.zss_cruise_active_last = control_available
|
||||
|
||||
# Compute ZSS offset once we have meaningful angles
|
||||
if self.zss_compute and abs(ret.steeringAngleDeg) > 1e-3 and abs(zorro_steer) > 1e-3:
|
||||
self.zss_compute = False
|
||||
self.zss_angle_offset = zorro_steer - ret.steeringAngleDeg
|
||||
|
||||
# Sanity checks
|
||||
steering_angle_deg = zorro_steer - self.zss_angle_offset
|
||||
if self.zss_threshold_count <= ZSS_MAX_THRESHOLD:
|
||||
if abs(ret.steeringAngleDeg - steering_angle_deg) > ZSS_DIFF_THRESHOLD:
|
||||
self.zss_threshold_count += 1
|
||||
else:
|
||||
ret.steeringAngleDeg = steering_angle_deg
|
||||
|
||||
# Update traffic signals and speed limit
|
||||
self.update_traffic_signals(cp_cam)
|
||||
ret_sp.speedLimit = self.calculate_speed_limit()
|
||||
@@ -0,0 +1,6 @@
|
||||
from opendbc.car.structs import CarParams
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
FW_VERSIONS_EXT = {
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.can_definitions import CanData
|
||||
from opendbc.car.toyota.values import CAR, MIN_ACC_SPEED, PEDAL_TRANSITION
|
||||
from opendbc.sunnypilot.car import create_gas_interceptor_command
|
||||
|
||||
|
||||
class GasInterceptorCarController:
|
||||
def __init__(self, CP, CP_SP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
self.gas = 0.
|
||||
self.interceptor_gas_cmd = 0.
|
||||
|
||||
def create_gas_command(self, CC: structs.CarControl, CS: structs.CarState, actuators: structs.CarControl.Actuators,
|
||||
packer, frame: int) -> list[CanData]:
|
||||
can_sends = []
|
||||
|
||||
if self.CP_SP.enableGasInterceptor and CC.longActive:
|
||||
MAX_INTERCEPTOR_GAS = 0.3
|
||||
# RAV4 has very sensitive gas pedal
|
||||
if self.CP.carFingerprint in (CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H, CAR.TOYOTA_HIGHLANDER):
|
||||
PEDAL_SCALE = np.interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
|
||||
elif self.CP.carFingerprint in (CAR.TOYOTA_COROLLA,):
|
||||
PEDAL_SCALE = np.interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
|
||||
else:
|
||||
PEDAL_SCALE = np.interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
|
||||
# offset for creep and windbrake
|
||||
pedal_offset = np.interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2])
|
||||
pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
|
||||
self.interceptor_gas_cmd = float(np.clip(pedal_command, 0., MAX_INTERCEPTOR_GAS))
|
||||
else:
|
||||
self.interceptor_gas_cmd = 0.
|
||||
|
||||
if frame % 2 == 0 and self.CP_SP.enableGasInterceptor and self.CP.openpilotLongitudinalControl:
|
||||
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
|
||||
# This prevents unexpected pedal range rescaling
|
||||
can_sends.append(create_gas_interceptor_command(packer, self.interceptor_gas_cmd, frame // 2))
|
||||
self.gas = self.interceptor_gas_cmd
|
||||
|
||||
return can_sends
|
||||
@@ -0,0 +1,22 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from enum import IntFlag
|
||||
|
||||
|
||||
class ToyotaFlagsSP(IntFlag):
|
||||
SMART_DSU = 1
|
||||
RADAR_CAN_FILTER = 2
|
||||
ZSS = 4
|
||||
STOCK_LONGITUDINAL = 8
|
||||
STOP_AND_GO_HACK = 16
|
||||
|
||||
|
||||
class ToyotaSafetyFlagsSP:
|
||||
DEFAULT = 0
|
||||
UNSUPPORTED_DSU = 1
|
||||
GAS_INTERCEPTOR = 2
|
||||
@@ -0,0 +1,25 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
from abc import abstractmethod, ABC
|
||||
from enum import StrEnum
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.can.parser import CANParser
|
||||
|
||||
|
||||
class MadsCarStateBase(ABC):
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP):
|
||||
self.CP = CP
|
||||
self.CP_SP = CP_SP
|
||||
|
||||
self.lkas_button = 0
|
||||
self.prev_lkas_button = 0
|
||||
|
||||
@abstractmethod
|
||||
def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
pass
|
||||
Reference in new issue
Block a user