IQ.Pilot Release Commit @ 2b39aa6
This commit is contained in:
IQ.Lvbs CI [bot]
@@ -1,14 +1,9 @@
|
||||
import re
|
||||
import json
|
||||
import os
|
||||
import unicodedata
|
||||
|
||||
from iqdbc.car.common.basedir import BASEDIR
|
||||
from iqdbc.car.docs import get_all_footnotes, get_params_for_docs
|
||||
from iqdbc.car.values import PLATFORMS
|
||||
|
||||
CAR_LIST_JSON_OUT = os.path.join(BASEDIR, "../", "iqpilot", "car", "car_list.json")
|
||||
|
||||
|
||||
def build_car_catalog() -> dict[str, dict[str, list[str] | str]]:
|
||||
collected_footnote = get_all_footnotes()
|
||||
@@ -62,8 +57,6 @@ def collect_car_docs(platforms, footnotes) -> dict[str, dict[str, list[str] | st
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
catalog = build_car_catalog()
|
||||
|
||||
with open(CAR_LIST_JSON_OUT, "w") as json_file:
|
||||
json.dump(catalog, json_file, indent=2, ensure_ascii=False)
|
||||
print(f"Generated and written to {CAR_LIST_JSON_OUT}")
|
||||
# build_car_catalog() is the raw platform source; the shipped catalog is generated
|
||||
# (and encoded to its on-disk envelope) by the main-repo entry point:
|
||||
print("run: python -m openpilot.iqpilot.selfdrive.car.vehicle_catalog")
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,3 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
@@ -1,21 +1,23 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
Always-on-Lateral adapter for Chrysler. CarState reads the LKAS toggle button
|
||||
(and the forwarded LKAS heartbeat); CarController tracks the AOL state and, when
|
||||
AOL is available, drives the LKAS_DISABLED bit the dash reads from the heartbeat.
|
||||
"""
|
||||
from enum import StrEnum
|
||||
from collections import namedtuple
|
||||
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.car.chrysler.values import RAM_CARS
|
||||
|
||||
from iqdbc.lvbs.aol_base import AolCarStateBase
|
||||
from iqdbc.can.parser import CANParser
|
||||
|
||||
AolDataIQ = namedtuple("AolDataIQ",
|
||||
["enable_aol", "paused", "lkas_disabled"])
|
||||
|
||||
AolDataIQ = namedtuple("AolDataIQ", ["enable_aol", "paused", "lkas_disabled"])
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
|
||||
_HEARTBIT_FIELDS = ("LKAS_DISABLED", "AUTO_HIGH_BEAM", "FORWARD_1", "FORWARD_2", "FORWARD_3")
|
||||
|
||||
|
||||
class AolCarController:
|
||||
def __init__(self):
|
||||
@@ -23,28 +25,18 @@ class AolCarController:
|
||||
|
||||
@staticmethod
|
||||
def create_lkas_heartbit(packer, lkas_heartbit, aol):
|
||||
# LKAS_HEARTBIT (0x2D9) LKAS heartbeat
|
||||
values = {s: lkas_heartbit[s] for s in [
|
||||
"LKAS_DISABLED",
|
||||
"AUTO_HIGH_BEAM",
|
||||
"FORWARD_1",
|
||||
"FORWARD_2",
|
||||
"FORWARD_3",
|
||||
]}
|
||||
|
||||
values = {name: lkas_heartbit[name] for name in _HEARTBIT_FIELDS}
|
||||
if aol.enable_aol:
|
||||
values["LKAS_DISABLED"] = 1 if aol.lkas_disabled else 0
|
||||
|
||||
return packer.make_can_msg("LKAS_HEARTBIT", 0, values)
|
||||
|
||||
@staticmethod
|
||||
def aol_status_update(CC: structs.CarControl, CC_IQ: structs.IQCarControl, CS) -> AolDataIQ:
|
||||
enable_aol = CC_IQ.aol.available
|
||||
paused = CC_IQ.aol.enabled and not CC.latActive
|
||||
|
||||
# A tap of the LKAS button flips the driver's "LKAS disabled" preference.
|
||||
if any(be.type == ButtonType.lkas and be.pressed for be in CS.out.buttonEvents):
|
||||
CS.lkas_disabled = not CS.lkas_disabled
|
||||
|
||||
return AolDataIQ(enable_aol, paused, CS.lkas_disabled)
|
||||
|
||||
def update(self, CC: structs.CarControl, CC_IQ: structs.IQCarControl, CS) -> None:
|
||||
@@ -55,35 +47,27 @@ class AolCarState(AolCarStateBase):
|
||||
def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams):
|
||||
super().__init__(CP, CP_IQ)
|
||||
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),
|
||||
]
|
||||
pt_messages.append(("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):
|
||||
def _read_lkas_button(self, cp, cp_cam) -> int:
|
||||
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 cp.vl["Center_Stack_2"]["LKAS_Button"]
|
||||
|
||||
return lkas_button
|
||||
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 cp.vl["TRACTION_BUTTON"]["TOGGLE_LKAS"]
|
||||
|
||||
def update_aol(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)
|
||||
self.lkas_button = self._read_lkas_button(can_parsers[Bus.pt], can_parsers[Bus.cam])
|
||||
|
||||
@@ -1,37 +0,0 @@
|
||||
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
from enum import StrEnum
|
||||
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.can.parser import CANParser
|
||||
from iqdbc.car.chrysler.values import RAM_HD
|
||||
from iqdbc.lvbs.car.chrysler.values_ext import BUTTONS
|
||||
|
||||
|
||||
class CarStateExt:
|
||||
def __init__(self, CP, CP_IQ):
|
||||
self.CP = CP
|
||||
self.CP_IQ = CP_IQ
|
||||
|
||||
self.button_events = []
|
||||
self.button_states = {button.event_type: False for button in BUTTONS}
|
||||
|
||||
def update(self, ret: structs.CarState, ret_iq: structs.IQCarState, 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"]
|
||||
@@ -1,23 +1,27 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
Chrysler low-speed steering gate: overrides the stock LKAS control bit when the
|
||||
no-min-steering-speed option is set, and holds the RAM DT engagement window.
|
||||
"""
|
||||
from iqdbc.car import structs
|
||||
from iqdbc.car.interfaces import CarStateBase
|
||||
from iqdbc.car.chrysler.values import RAM_DT
|
||||
from iqdbc.lvbs.car.chrysler.values_ext import ChryslerFlagsIQ
|
||||
from iqdbc.lvbs.car.chrysler.iq_values import ChryslerFlagsIQ
|
||||
|
||||
GearShifter = structs.CarState.GearShifter
|
||||
|
||||
|
||||
class CarControllerExt:
|
||||
class IQCarController:
|
||||
def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams):
|
||||
self.CP = CP
|
||||
self.CP_IQ = CP_IQ
|
||||
|
||||
def get_lkas_control_bit(self, CS: CarStateBase, CC: structs.CarControl, lkas_control_bit: bool) -> bool:
|
||||
if self.CP_IQ.flags & ChryslerFlagsIQ.NO_MIN_STEERING_SPEED:
|
||||
lkas_control_bit = CC.latActive
|
||||
elif self.CP.carFingerprint in RAM_DT:
|
||||
return CC.latActive
|
||||
|
||||
if 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:
|
||||
32
iqdbc_repo/iqdbc/lvbs/car/chrysler/iq_carstate.py
Normal file
32
iqdbc_repo/iqdbc/lvbs/car/chrysler/iq_carstate.py
Normal file
@@ -0,0 +1,32 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
Chrysler cruise-button reader: emits edge ButtonEvents for the ACC steering-wheel
|
||||
buttons so IQ.Pilot can react to accel/decel/cancel/resume.
|
||||
"""
|
||||
from enum import StrEnum
|
||||
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.can.parser import CANParser
|
||||
from iqdbc.lvbs.car.chrysler.iq_values import BUTTONS
|
||||
|
||||
|
||||
class IQCarState:
|
||||
def __init__(self, CP, CP_IQ):
|
||||
self.CP = CP
|
||||
self.CP_IQ = CP_IQ
|
||||
self.button_events: list = []
|
||||
self.button_states = {button.event_type: False for button in BUTTONS}
|
||||
|
||||
def update(self, ret: structs.CarState, ret_iq: structs.IQCarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
cp = can_parsers[Bus.pt]
|
||||
events = []
|
||||
for button in BUTTONS:
|
||||
pressed = cp.vl[button.can_addr][button.can_msg] in button.values
|
||||
if pressed != self.button_states[button.event_type]:
|
||||
event = structs.CarState.ButtonEvent.new_message()
|
||||
event.type = button.event_type
|
||||
event.pressed = pressed
|
||||
events.append(event)
|
||||
self.button_states[button.event_type] = pressed
|
||||
self.button_events = events
|
||||
@@ -1,7 +1,9 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
IQ.Pilot Chrysler extension flags + the cruise-button table read by the AOL
|
||||
cruise-button reader.
|
||||
"""
|
||||
from collections import namedtuple
|
||||
from enum import IntFlag
|
||||
|
||||
@@ -20,5 +22,3 @@ BUTTONS = [
|
||||
|
||||
class ChryslerFlagsIQ(IntFlag):
|
||||
NO_MIN_STEERING_SPEED = 1
|
||||
|
||||
|
||||
@@ -1,3 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
@@ -1,21 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
from enum import StrEnum
|
||||
|
||||
from iqdbc.car import Bus,structs
|
||||
|
||||
from iqdbc.lvbs.aol_base import AolCarStateBase
|
||||
from iqdbc.can.parser import CANParser
|
||||
|
||||
|
||||
class AolCarState(AolCarStateBase):
|
||||
def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams):
|
||||
super().__init__(CP, CP_IQ)
|
||||
|
||||
def update_aol(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"]
|
||||
@@ -1,3 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
@@ -1,58 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import os
|
||||
from math import exp
|
||||
|
||||
from iqdbc.car import structs
|
||||
from iqdbc.car.common.basedir import BASEDIR
|
||||
from iqdbc.car.gm.interface import CAR
|
||||
from iqdbc.lvbs.car.interfaces import LatControlInputs, NanoFFModel, TorqueFromLateralAccelCallbackTypeTorqueSpace
|
||||
|
||||
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:
|
||||
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
|
||||
@@ -1,23 +1,26 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
Non-ACC GM carState: these cars have no adaptive cruise, so cruise engage/set
|
||||
speed come from the stock (non-adaptive) ECM cruise message.
|
||||
"""
|
||||
from enum import StrEnum
|
||||
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.can.parser import CANParser
|
||||
from iqdbc.lvbs.car.gm.values_ext import GMFlagsIQ
|
||||
from iqdbc.lvbs.car.gm.iq_values import GMFlagsIQ
|
||||
|
||||
|
||||
class CarStateExt:
|
||||
class IQCarState:
|
||||
def __init__(self, CP, CP_IQ):
|
||||
self.CP = CP
|
||||
self.CP_IQ = CP_IQ
|
||||
|
||||
def update(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
if not self.CP_IQ.flags & GMFlagsIQ.NON_ACC:
|
||||
return
|
||||
pt_cp = can_parsers[Bus.pt]
|
||||
|
||||
if self.CP_IQ.flags & GMFlagsIQ.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
|
||||
ret.cruiseState.enabled = pt_cp.vl["ECMCruiseControl"]["CruiseActive"] != 0
|
||||
ret.cruiseState.speed = pt_cp.vl["ECMCruiseControl"]["CruiseSetSpeed"] * CV.KPH_TO_MS
|
||||
ret.accFaulted = False
|
||||
43
iqdbc_repo/iqdbc/lvbs/car/gm/iq_interface.py
Normal file
43
iqdbc_repo/iqdbc/lvbs/car/gm/iq_interface.py
Normal file
@@ -0,0 +1,43 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
GM torque-space feed-forward for the IQ.Pilot lateral extension: a sigmoid+linear
|
||||
lat-accel -> torque curve for the tuned platforms, else the linear default.
|
||||
"""
|
||||
from math import exp
|
||||
|
||||
from iqdbc.car import structs
|
||||
from iqdbc.car.gm.interface import CAR
|
||||
from iqdbc.lvbs.car.interfaces import LatControlInputs, TorqueFromLateralAccelCallbackTypeTorqueSpace
|
||||
|
||||
_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 IQCarInterface:
|
||||
def __init__(self, CP: structs.CarParams, CI_Base):
|
||||
self.CP = CP
|
||||
self.CI_Base = CI_Base
|
||||
|
||||
@staticmethod
|
||||
def _centered_sigmoid(val: float) -> float:
|
||||
# sigmoid shifted to pass through the origin; branch keeps exp() from overflowing
|
||||
if val >= 0:
|
||||
return 1.0 / (1.0 + exp(-val)) - 0.5
|
||||
z = exp(val)
|
||||
return z / (1.0 + z) - 0.5
|
||||
|
||||
def torque_from_lateral_accel_siglin(self, latcontrol_inputs: LatControlInputs,
|
||||
torque_params: structs.CarParams.LateralTorqueTuning,
|
||||
gravity_adjusted: bool) -> float:
|
||||
a, b, c, _ = _NON_LINEAR_TORQUE_PARAMS[self.CP.carFingerprint]
|
||||
lat_accel = latcontrol_inputs.lateral_acceleration
|
||||
return float(self._centered_sigmoid(lat_accel * a) * b + lat_accel * c)
|
||||
|
||||
def torque_from_lateral_accel_in_torque_space(self) -> TorqueFromLateralAccelCallbackTypeTorqueSpace:
|
||||
if self.CP.carFingerprint in _NON_LINEAR_TORQUE_PARAMS:
|
||||
return self.torque_from_lateral_accel_siglin
|
||||
return self.CI_Base.torque_from_lateral_accel_linear_in_torque_space
|
||||
@@ -1,7 +1,8 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
IQ.Pilot GM extension flags for the non-adaptive-cruise (Non-ACC) camera-harness port.
|
||||
"""
|
||||
from enum import IntFlag
|
||||
|
||||
|
||||
@@ -11,5 +12,3 @@ class GMFlagsIQ(IntFlag):
|
||||
|
||||
class GMSafetyFlagsIQ:
|
||||
NON_ACC = 1
|
||||
|
||||
|
||||
@@ -5,10 +5,10 @@ from enum import StrEnum
|
||||
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.can.parser import CANParser
|
||||
from iqdbc.lvbs.car.honda.values_ext import HondaFlagsIQ
|
||||
from iqdbc.lvbs.car.honda.iq_values import HondaFlagsIQ
|
||||
|
||||
|
||||
class CarStateExt:
|
||||
class IQCarState:
|
||||
def __init__(self, CP, CP_IQ):
|
||||
self.CP = CP
|
||||
self.CP_IQ = CP_IQ
|
||||
@@ -1,3 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
@@ -1,105 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
from enum import StrEnum
|
||||
from collections import namedtuple
|
||||
|
||||
from iqdbc.car import Bus, DT_CTRL, structs
|
||||
from iqdbc.car.hyundai.values import CAR
|
||||
|
||||
from iqdbc.car.hyundai.values import HyundaiFlags
|
||||
from iqdbc.lvbs.car.hyundai.values import HyundaiFlagsIQ
|
||||
from iqdbc.lvbs.aol_base import AolCarStateBase
|
||||
from iqdbc.can.parser import CANParser
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
|
||||
AolDataIQ = namedtuple("AolDataIQ",
|
||||
["enable_aol", "lat_active", "disengaging", "paused"])
|
||||
|
||||
|
||||
class AolCarController:
|
||||
def __init__(self):
|
||||
self.aol = AolDataIQ(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 aol_status_update(self, CC: structs.CarControl, CC_IQ: structs.IQCarControl, frame: int) -> AolDataIQ:
|
||||
enable_aol = CC_IQ.aol.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_IQ.aol.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 AolDataIQ(enable_aol, CC.latActive, disengaging, paused)
|
||||
|
||||
def create_lkas_icon(self, CP: structs.CarParams, enabled: bool) -> int:
|
||||
if self.aol.enable_aol:
|
||||
lkas_icon = 2 if self.aol.lat_active else 3 if self.aol.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.aol.lat_active if self.aol.enable_aol else enabled) else 1
|
||||
|
||||
return lkas_icon
|
||||
|
||||
def create_lfa_icon(self, enabled: bool) -> int:
|
||||
if self.aol.enable_aol:
|
||||
lfa_icon = 2 if self.aol.lat_active else 3 if self.aol.disengaging else 1 if self.aol.paused else 0
|
||||
else:
|
||||
lfa_icon = 2 if enabled else 0
|
||||
|
||||
return lfa_icon
|
||||
|
||||
def update(self, CP: structs.CarParams, CC: structs.CarControl, CC_IQ: structs.IQCarControl, frame: int) -> None:
|
||||
self.aol = self.aol_status_update(CC, CC_IQ, frame)
|
||||
self.lkas_icon = self.create_lkas_icon(CP, CC.enabled)
|
||||
self.lfa_icon = self.create_lfa_icon(CC.enabled)
|
||||
|
||||
|
||||
class AolCarState(AolCarStateBase):
|
||||
def __init__(self, CP: structs.CarParams, CP_IQ: structs.CarParams):
|
||||
super().__init__(CP, CP_IQ)
|
||||
self.main_cruise_enabled: bool = False
|
||||
|
||||
@staticmethod
|
||||
def get_parser(CP, CP_IQ, pt_messages) -> None:
|
||||
pass
|
||||
|
||||
def get_main_cruise(self, ret: structs.CarState) -> bool:
|
||||
if self.CP_IQ.flags & HyundaiFlagsIQ.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_aol(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
pass
|
||||
|
||||
def update_aol_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
|
||||
@@ -1,83 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
from enum import StrEnum
|
||||
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.can.parser import CANParser
|
||||
from iqdbc.car.hyundai.values import HyundaiFlags
|
||||
from iqdbc.lvbs.car.hyundai.values import HyundaiFlagsIQ
|
||||
|
||||
|
||||
class CarStateExt:
|
||||
def __init__(self, CP, CP_IQ):
|
||||
self.CP = CP
|
||||
self.CP_IQ = CP_IQ
|
||||
|
||||
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_IQ.flags & HyundaiFlagsIQ.SPEED_LIMIT_AVAILABLE:
|
||||
bus = cp if self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING else cp_cam
|
||||
speed_limit = bus.vl["FR_CMR_02_100ms"]["ISLW_SpdCluMainDis"]
|
||||
else:
|
||||
nav, cam = 0, 0
|
||||
if self.CP_IQ.flags & HyundaiFlagsIQ.SPEED_LIMIT_AVAILABLE:
|
||||
nav = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"]
|
||||
if self.CP_IQ.flags & HyundaiFlagsIQ.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_iq: structs.IQCarState, 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_IQ.flags & HyundaiFlagsIQ.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_IQ.flags & HyundaiFlagsIQ.NON_SCC_NO_FCA:
|
||||
cp_cruise = cp if self.CP_IQ.flags & HyundaiFlagsIQ.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_iq.speedLimit = self.update_speed_limit(cp, cp_cam) * speed_conv
|
||||
|
||||
def update_canfd_ext(self, ret: structs.CarState, ret_iq: structs.IQCarState, 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_iq.speedLimit = self.update_speed_limit(cp, cp_cam) * speed_factor
|
||||
@@ -1,71 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
from iqdbc.car import uds
|
||||
from iqdbc.car.carlog import carlog
|
||||
from iqdbc.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 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}")
|
||||
@@ -1,68 +0,0 @@
|
||||
from iqdbc.car import structs
|
||||
|
||||
from iqdbc.lvbs.car.hyundai.values import HyundaiFlagsIQ
|
||||
|
||||
ESCC_MSG = 0x2AB
|
||||
|
||||
|
||||
class EnhancedSmartCruiseControl:
|
||||
def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams):
|
||||
self.CP = CP
|
||||
self.CP_IQ = CP_IQ
|
||||
|
||||
@property
|
||||
def enabled(self):
|
||||
return self.CP_IQ.flags & HyundaiFlagsIQ.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-IQ: 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_IQ: structs.IQCarParams):
|
||||
self.ESCC = EnhancedSmartCruiseControl(CP, CP_IQ)
|
||||
|
||||
def update(self, car_state):
|
||||
self.ESCC.update_car_state(car_state)
|
||||
|
||||
|
||||
class EsccRadarInterfaceBase:
|
||||
def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams):
|
||||
self.ESCC = EnhancedSmartCruiseControl(CP, CP_IQ)
|
||||
self.use_escc = False
|
||||
@@ -1,126 +0,0 @@
|
||||
from iqdbc.car.structs import CarParams
|
||||
from iqdbc.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-IQ: 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'
|
||||
],
|
||||
},
|
||||
}
|
||||
@@ -1,114 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
from abc import ABC, abstractmethod
|
||||
from iqdbc.car import structs
|
||||
from iqdbc.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_IQ: structs.IQCarControl) -> None:
|
||||
self.lead_one = CC_IQ.leadOne
|
||||
self.lead_two = CC_IQ.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)
|
||||
@@ -1,3 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
@@ -1,65 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from iqdbc.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: [5., 20.])
|
||||
lookahead_jerk_upper_v: list[float] = field(default_factory=lambda: [0.25, 0.5])
|
||||
lookahead_jerk_lower_v: list[float] = field(default_factory=lambda: [0.15, 0.3])
|
||||
longitudinal_actuator_delay: float = 0.45
|
||||
jerk_limits: float = 4.0
|
||||
|
||||
|
||||
# Default configurations for different car types
|
||||
TUNING_CONFIGS = {
|
||||
"CANFD": CarTuningConfig(
|
||||
v_ego_stopping=0.365,
|
||||
lookahead_jerk_bp=[2., 5., 20.],
|
||||
lookahead_jerk_upper_v=[0.25, 0.5, 1.0],
|
||||
lookahead_jerk_lower_v=[0.05, 0.10, 0.325],
|
||||
),
|
||||
"EV": CarTuningConfig(
|
||||
stopping_decel_rate=0.45,
|
||||
v_ego_stopping=0.35,
|
||||
lookahead_jerk_upper_v=[0.3, 0.7],
|
||||
lookahead_jerk_lower_v=[0.2, 0.4],
|
||||
),
|
||||
"HYBRID": CarTuningConfig(
|
||||
v_ego_starting=0.15,
|
||||
stopping_decel_rate=0.45,
|
||||
v_ego_stopping=0.4,
|
||||
),
|
||||
"DEFAULT": CarTuningConfig(
|
||||
lookahead_jerk_bp=[2., 5., 20.],
|
||||
lookahead_jerk_upper_v=[0.25, 0.5, 1.0],
|
||||
lookahead_jerk_lower_v=[0.05, 0.10, 0.3],
|
||||
)
|
||||
}
|
||||
|
||||
# Car-specific configs
|
||||
CAR_SPECIFIC_CONFIGS = {
|
||||
CAR.KIA_NIRO_EV: CarTuningConfig(
|
||||
stopping_decel_rate=0.3,
|
||||
lookahead_jerk_upper_v=[0.3, 1.0],
|
||||
lookahead_jerk_lower_v=[0.2, 0.4],
|
||||
jerk_limits=2.5,
|
||||
),
|
||||
CAR.KIA_NIRO_PHEV_2022: CarTuningConfig(
|
||||
stopping_decel_rate=0.3,
|
||||
lookahead_jerk_upper_v=[0.3, 1.0],
|
||||
lookahead_jerk_lower_v=[0.15, 0.3],
|
||||
jerk_limits=4.0,
|
||||
),
|
||||
CAR.HYUNDAI_IONIQ: CarTuningConfig(
|
||||
jerk_limits=4.5,
|
||||
)
|
||||
}
|
||||
@@ -1,292 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from dataclasses import dataclass
|
||||
|
||||
from iqdbc.car import structs, DT_CTRL
|
||||
from iqdbc.car.interfaces import CarStateBase
|
||||
from iqdbc.car.hyundai.values import CarControllerParams
|
||||
from iqdbc.lvbs.car.hyundai.longitudinal.helpers import get_car_config, jerk_limited_integrator, ramp_update
|
||||
from iqdbc.lvbs.car.hyundai.values import HyundaiFlagsIQ
|
||||
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
|
||||
MIN_JERK = 0.5
|
||||
COMFORT_BAND_VAL = 0.01
|
||||
|
||||
DYNAMIC_LOWER_JERK_BP = [-2.0, -1.5, -1.0, -0.25, -0.1, -0.025, -0.01, -0.005]
|
||||
DYNAMIC_LOWER_JERK_V = [3.3, 2.5, 2.0, 1.9, 1.8, 1.65, 1.15, 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_IQ: structs.IQCarParams) -> None:
|
||||
self.CP = CP
|
||||
self.CP_IQ = CP_IQ
|
||||
|
||||
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_IQ.flags & (HyundaiFlagsIQ.LONG_TUNING_DYNAMIC | HyundaiFlagsIQ.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 * 2):
|
||||
self.stopping = True
|
||||
|
||||
self.stopping_count += 1
|
||||
|
||||
@staticmethod
|
||||
def _calculate_speed_based_jerk_limits(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³
|
||||
"""
|
||||
|
||||
# Upper jerk limit varies based on speed and control state
|
||||
if long_control_state == LongCtrlState.pid:
|
||||
upper_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [2.0, 3.0, 1.6]))
|
||||
else:
|
||||
upper_limit = 0.5 # Default for non-PID states
|
||||
|
||||
# Lower jerk limit varies based on speed
|
||||
lower_limit = float(np.interp(velocity, [0.0, 5.0, 20.0], [5.0, 4.0, 2.5]))
|
||||
|
||||
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
|
||||
|
||||
return j_ego_upper, j_ego_lower
|
||||
|
||||
def _calculate_dynamic_lower_jerk(self, accel_error: float, velocity: float) -> float:
|
||||
"""Calculate dynamic jerk for braking based on acceleration error.
|
||||
|
||||
Used for the dynamic tuning approach (non-predictive).
|
||||
|
||||
Args:
|
||||
accel_error: Difference between actual and previous acceleration (m/s²)
|
||||
velocity: Current vehicle speed (m/s)
|
||||
|
||||
Returns:
|
||||
Dynamic lower jerk limit (m/s³)
|
||||
"""
|
||||
|
||||
if self.CP.radarUnavailable:
|
||||
return 5.0
|
||||
|
||||
if accel_error < 0:
|
||||
# Scale the brake jerk values based on car config
|
||||
lower_max = self.car_config.jerk_limits
|
||||
original_values = np.array(DYNAMIC_LOWER_JERK_V)
|
||||
scaled_values = original_values * (lower_max / original_values[0])
|
||||
|
||||
# Interpolate based on acceleration error
|
||||
dynamic_lower_jerk = float(np.interp(accel_error, DYNAMIC_LOWER_JERK_BP, scaled_values))
|
||||
else:
|
||||
dynamic_lower_jerk = 0.5
|
||||
|
||||
return dynamic_lower_jerk
|
||||
|
||||
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)
|
||||
|
||||
# Calculate lookahead jerk
|
||||
j_ego_upper, j_ego_lower = self._calculate_lookahead_jerk(accel_error, velocity)
|
||||
|
||||
# Calculate lower jerk limit
|
||||
lower_jerk = max(-j_ego_lower, MIN_JERK)
|
||||
if self.CP.radarUnavailable:
|
||||
lower_jerk = 5.0
|
||||
|
||||
# Final jerk limits with thresholds
|
||||
desired_jerk_upper = min(max(j_ego_upper, MIN_JERK), upper_speed_factor)
|
||||
desired_jerk_lower = min(lower_jerk, lower_speed_factor)
|
||||
|
||||
# Calculate dynamic lower jerk for non-predictive tuning
|
||||
a_ego_blended = float(np.interp(velocity, [1.0, 2.0], [CS.aBasis, CS.out.aEgo]))
|
||||
dynamic_accel_error = a_ego_blended - self.accel_last
|
||||
dynamic_lower_jerk = self._calculate_dynamic_lower_jerk(dynamic_accel_error, velocity)
|
||||
dynamic_desired_lower_jerk = min(dynamic_lower_jerk, lower_speed_factor)
|
||||
|
||||
# Apply jerk limits based on tuning approach
|
||||
self.jerk_upper = ramp_update(self.jerk_upper, desired_jerk_upper)
|
||||
|
||||
# Predictive tuning uses calculated desired jerk directly
|
||||
# Dynamic tuning applies a ramped approach for smoother transitions
|
||||
if self.CP_IQ.flags & HyundaiFlagsIQ.LONG_TUNING_PREDICTIVE:
|
||||
self.jerk_lower = desired_jerk_lower
|
||||
else:
|
||||
self.jerk_lower = ramp_update(self.jerk_lower, dynamic_desired_lower_jerk)
|
||||
|
||||
# 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 or self.CP.radarUnavailable:
|
||||
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
|
||||
self.actual_accel = jerk_limited_integrator(self.desired_accel, self.accel_last, self.jerk_upper, self.jerk_lower)
|
||||
|
||||
self.accel_last = self.actual_accel
|
||||
|
||||
def calculate_comfort_band(self, CC: structs.CarControl) -> None:
|
||||
if not self.enabled or self.CP.radarUnavailable or not CC.longActive:
|
||||
self.comfort_band_upper = 0.0
|
||||
self.comfort_band_lower = 0.0
|
||||
return
|
||||
|
||||
self.comfort_band_upper = COMFORT_BAND_VAL
|
||||
self.comfort_band_lower = COMFORT_BAND_VAL
|
||||
|
||||
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)
|
||||
self.get_tuning_state()
|
||||
|
||||
self.long_control_state_last = long_control_state
|
||||
@@ -1,60 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
from iqdbc.car import structs, DT_CTRL, rate_limit
|
||||
from iqdbc.car.hyundai.values import HyundaiFlags
|
||||
from iqdbc.lvbs.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 * 2
|
||||
else:
|
||||
val = jerk_lower * DT_CTRL * 2
|
||||
|
||||
return rate_limit(desired_accel, last_accel, -val, val)
|
||||
|
||||
|
||||
def ramp_update(current, target):
|
||||
error = target - current
|
||||
if abs(error) > JERK_THRESHOLD:
|
||||
return current + float(np.clip(error, -JERK_STEP, JERK_STEP))
|
||||
return target
|
||||
@@ -1,85 +0,0 @@
|
||||
from iqdbc.can.parser import CANParser
|
||||
from iqdbc.car import structs, Bus
|
||||
from iqdbc.car.hyundai.hyundaicanfd import CanBus
|
||||
from iqdbc.car.hyundai.values import DBC, HyundaiFlags
|
||||
|
||||
from iqdbc.lvbs.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_IQ: structs.IQCarParams):
|
||||
EsccRadarInterfaceBase.__init__(self, CP, CP_IQ)
|
||||
self.CP = CP
|
||||
self.CP_IQ = CP_IQ
|
||||
|
||||
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].measured = True
|
||||
self.pts[ii].dRel = msg['ACC_ObjDist']
|
||||
self.pts[ii].yRel = float('nan') # FIXME-IQ: Only some cars have lateral position from SCC
|
||||
self.pts[ii].vRel = msg['ACC_ObjRelSpd']
|
||||
self.pts[ii].aRel = float('nan') # TODO-IQ: calculate from ACC_ObjRelSpd and with timestep 50Hz (needs to modify in interfaces.py)
|
||||
self.pts[ii].yvRel = float('nan')
|
||||
|
||||
else:
|
||||
del self.pts[ii]
|
||||
|
||||
ret.points = list(self.pts.values())
|
||||
return ret
|
||||
@@ -1,3 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
@@ -1,61 +0,0 @@
|
||||
import pytest
|
||||
from hypothesis import given, strategies as st, settings, HealthCheck
|
||||
from iqdbc.lvbs.car.hyundai.escc import EnhancedSmartCruiseControl, ESCC_MSG
|
||||
from iqdbc.car.hyundai.carstate import CarState
|
||||
from iqdbc.car import structs
|
||||
from iqdbc.lvbs.car.hyundai.values import HyundaiFlagsIQ
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def car_params():
|
||||
params = structs.CarParams()
|
||||
params.carFingerprint = "HYUNDAI_SONATA"
|
||||
return params
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def car_params_iq():
|
||||
params = structs.IQCarParams()
|
||||
params.flags = HyundaiFlagsIQ.ENHANCED_SCC
|
||||
return params
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def escc(car_params, car_params_iq):
|
||||
return EnhancedSmartCruiseControl(car_params, car_params_iq)
|
||||
|
||||
|
||||
class TestEscc:
|
||||
def test_escc_msg_id(self, escc):
|
||||
assert escc.trigger_msg == ESCC_MSG
|
||||
|
||||
@settings(suppress_health_check=[HealthCheck.function_scoped_fixture])
|
||||
@given(st.integers(min_value=0, max_value=255))
|
||||
def test_enabled_flag(self, car_params, car_params_iq, value):
|
||||
car_params_iq.flags = value
|
||||
escc = EnhancedSmartCruiseControl(car_params, car_params_iq)
|
||||
assert escc.enabled == (value & HyundaiFlagsIQ.ENHANCED_SCC)
|
||||
|
||||
def test_update_car_state(self, escc, car_params, car_params_iq):
|
||||
car_state = CarState(car_params, car_params_iq)
|
||||
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)
|
||||
assert escc.car_state == car_state
|
||||
|
||||
def test_update_scc12(self, escc, car_params, car_params_iq):
|
||||
car_state = CarState(car_params, car_params_iq)
|
||||
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)
|
||||
assert scc12_message["AEB_CmdAct"] == 1
|
||||
assert scc12_message["CF_VSM_Warn"] == 1
|
||||
assert scc12_message["CF_VSM_DecCmdAct"] == 1
|
||||
assert scc12_message["CR_VSM_DecCmd"] == 1
|
||||
assert scc12_message["AEB_Status"] == 2
|
||||
@@ -1,77 +0,0 @@
|
||||
from enum import IntFlag
|
||||
|
||||
from iqdbc.lvbs.car.hyundai.lead_data_ext import LeadDataCarController, CanLeadData, CanFdLeadData
|
||||
from iqdbc.car import structs
|
||||
from iqdbc.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_iq_carcontrol(leadDistance=10.0, leadRelSpeed=0.0, leadVisible=True):
|
||||
c = structs.IQCarControl()
|
||||
c.leadOne.dRel = leadDistance
|
||||
c.leadOne.vRel = leadRelSpeed
|
||||
c.leadOne.status = leadVisible
|
||||
return c
|
||||
|
||||
|
||||
class TestLeadDataCarController:
|
||||
def test_update_object_gap(self):
|
||||
ctrl = LeadDataCarController(make_carparams())
|
||||
# Initial value should be 0
|
||||
assert ctrl.object_gap == 0
|
||||
|
||||
# Set to 15 (should become 2 after hysteresis)
|
||||
for _ in range(ctrl.LEAD_HYSTERESIS_FRAMES):
|
||||
ctrl._update_object_gap(15)
|
||||
assert ctrl.object_gap == 2
|
||||
|
||||
# Set to 22 (should become 3 after hysteresis)
|
||||
for _ in range(ctrl.LEAD_HYSTERESIS_FRAMES):
|
||||
ctrl._update_object_gap(22)
|
||||
assert ctrl.object_gap == 3
|
||||
|
||||
# Set to 0 (should become 0 after hysteresis)
|
||||
for _ in range(ctrl.LEAD_HYSTERESIS_FRAMES):
|
||||
ctrl._update_object_gap(0)
|
||||
assert ctrl.object_gap == 0
|
||||
|
||||
def test_update_lead_visible_hysteresis(self):
|
||||
ctrl = LeadDataCarController(make_carparams())
|
||||
ctrl._update_lead_visible_hysteresis(True)
|
||||
assert isinstance(ctrl.lead_visible, bool)
|
||||
ctrl._update_lead_visible_hysteresis(False)
|
||||
assert isinstance(ctrl.lead_visible, bool)
|
||||
|
||||
def test_update(self):
|
||||
ctrl = LeadDataCarController(make_carparams())
|
||||
iq_control = make_iq_carcontrol(leadDistance=25, leadRelSpeed=-0.5, leadVisible=True)
|
||||
ctrl.update(iq_control)
|
||||
assert ctrl.lead_distance == 25
|
||||
assert ctrl.lead_rel_speed == -0.5
|
||||
assert isinstance(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
|
||||
assert isinstance(ld, CanLeadData)
|
||||
assert 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
|
||||
assert isinstance(ld, CanFdLeadData)
|
||||
assert ld.object_rel_gap == 1
|
||||
@@ -1,119 +0,0 @@
|
||||
from parameterized import parameterized
|
||||
|
||||
from iqdbc.car import CanData
|
||||
from iqdbc.car.car_helpers import interfaces
|
||||
from iqdbc.car.hyundai.values import CAR, HyundaiFlags
|
||||
from iqdbc.lvbs.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:
|
||||
|
||||
@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_IQ = CarInterface.get_non_essential_params_iq(CP, car_name)
|
||||
|
||||
CI = CarInterface(CP, CP_IQ)
|
||||
|
||||
RD = CI.RadarInterface(CP, CP_IQ)
|
||||
|
||||
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_IQ
|
||||
|
||||
@parameterized.expand(ESCC_CARS)
|
||||
def test_escc_radar_interface(self, car_name, escc_msg):
|
||||
"""Test radar interface for ESCC-enabled cars"""
|
||||
RD, CP, CP_IQ = self._setup_platform(car_name, escc_msg=escc_msg)
|
||||
|
||||
# Assert that ESCC features are present
|
||||
if hasattr(RD, 'use_escc'):
|
||||
assert RD.use_escc, "ESCC car should have use_escc=True"
|
||||
if hasattr(RD, 'use_radar_interface_ext'):
|
||||
assert 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)
|
||||
assert rr is None or len(rr.errors) > 0
|
||||
|
||||
@parameterized.expand(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_IQ = self._setup_platform(car_name, additional_flags=flags)
|
||||
|
||||
# Assert Camera SCC flag is set appropriately
|
||||
if flags & HyundaiFlags.CAMERA_SCC:
|
||||
assert CP.flags & HyundaiFlags.CAMERA_SCC, "Car should have CAMERA_SCC flag"
|
||||
if flags & HyundaiFlags.CANFD_CAMERA_SCC:
|
||||
assert 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'):
|
||||
assert RD.use_radar_interface_ext, "Camera SCC car should use radar interface ext"
|
||||
|
||||
# Verify trigger message
|
||||
if hasattr(RD, 'trigger_msg'):
|
||||
assert 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)
|
||||
assert rr is None or len(rr.errors) > 0
|
||||
|
||||
@parameterized.expand(STANDARD_RADAR_CARS)
|
||||
def test_standard_radar_interface(self, car_name, flags):
|
||||
"""Test radar interface for standard radar cars"""
|
||||
RD, CP, CP_IQ = self._setup_platform(car_name, additional_flags=flags)
|
||||
|
||||
# Standard cars should not use radar interface ext
|
||||
if hasattr(RD, 'use_radar_interface_ext'):
|
||||
assert not 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)
|
||||
assert rr is None or len(rr.errors) > 0
|
||||
@@ -1,146 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
import unittest
|
||||
import numpy as np
|
||||
from unittest.mock import Mock
|
||||
|
||||
from iqdbc.lvbs.car.hyundai.longitudinal.controller import LongitudinalController, LongitudinalState
|
||||
from iqdbc.lvbs.car.hyundai.values import HyundaiFlagsIQ
|
||||
from iqdbc.car import DT_CTRL, structs
|
||||
from iqdbc.car.interfaces import CarStateBase
|
||||
from iqdbc.car.hyundai.values import HyundaiFlags
|
||||
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
|
||||
|
||||
class TestLongitudinalTuningController(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.mock_CP = Mock(carFingerprint="KIA_NIRO_EV", flags=0)
|
||||
self.mock_CP.radarUnavailable = False # ensure tuning branch
|
||||
self.mock_CP_IQ = Mock(flags=0)
|
||||
self.controller = LongitudinalController(self.mock_CP, self.mock_CP_IQ)
|
||||
|
||||
def test_init(self):
|
||||
"""Test controller initialization"""
|
||||
self.assertIsInstance(self.controller.tuning, LongitudinalState)
|
||||
self.assertEqual(self.controller.desired_accel, 0.0)
|
||||
self.assertEqual(self.controller.actual_accel, 0.0)
|
||||
self.assertEqual(self.controller.jerk_upper, 0.0)
|
||||
self.assertEqual(self.controller.jerk_lower, 0.0)
|
||||
self.assertEqual(self.controller.comfort_band_upper, 0.0)
|
||||
self.assertEqual(self.controller.comfort_band_lower, 0.0)
|
||||
|
||||
def test_make_jerk_flag_off(self):
|
||||
"""Test when LONG_TUNING_DYNAMIC flag is off"""
|
||||
mock_CC, mock_CS = Mock(spec=structs.CarControl), Mock(spec=CarStateBase)
|
||||
mock_CS.out = Mock()
|
||||
mock_CS.out.vEgo = 0.0
|
||||
mock_CS.out.aEgo = 0.0
|
||||
mock_CS.aBasis = 0.0
|
||||
|
||||
# Test with PID state
|
||||
self.controller.calculate_jerk(mock_CC, mock_CS, LongCtrlState.pid)
|
||||
print(f"[PID state] jerk_upper={self.controller.jerk_upper:.2f}, jerk_lower={self.controller.jerk_lower:.2f}")
|
||||
self.assertEqual(self.controller.jerk_upper, 3.0)
|
||||
self.assertEqual(self.controller.jerk_lower, 5.0)
|
||||
|
||||
# Test with non-PID state
|
||||
self.controller.calculate_jerk(mock_CC, mock_CS, LongCtrlState.stopping)
|
||||
print(f"[Non-PID state] jerk_upper={self.controller.jerk_upper:.2f}, jerk_lower={self.controller.jerk_lower:.2f}")
|
||||
self.assertEqual(self.controller.jerk_upper, 1.0)
|
||||
self.assertEqual(self.controller.jerk_lower, 5.0)
|
||||
|
||||
def test_make_jerk_flag_on(self):
|
||||
"""Only verify that limits update when flags are on."""
|
||||
self.controller.CP_IQ.flags = HyundaiFlagsIQ.LONG_TUNING_DYNAMIC
|
||||
self.controller.CP.flags = HyundaiFlags.CANFD
|
||||
mock_CC = Mock()
|
||||
mock_CC.actuators = Mock(accel=1.0)
|
||||
mock_CC.longActive = True
|
||||
self.controller.stopping = False
|
||||
mock_CS = Mock()
|
||||
mock_CS.out = Mock(aEgo=0.8, vEgo=3.0)
|
||||
mock_CS.aBasis = 0.8
|
||||
|
||||
self.controller.calculate_jerk(mock_CC, mock_CS, LongCtrlState.pid)
|
||||
print(f"[FlagOn] jerk_upper={self.controller.jerk_upper:.3f}, jerk_lower={self.controller.jerk_lower:.3f}")
|
||||
self.assertGreater(self.controller.jerk_upper, 0.0)
|
||||
self.assertGreater(self.controller.jerk_lower, 0.0)
|
||||
|
||||
def test_a_value_jerk_scaling(self):
|
||||
"""Test a_value jerk scaling under tuning branch."""
|
||||
self.controller.CP_IQ.flags = HyundaiFlagsIQ.LONG_TUNING_DYNAMIC
|
||||
self.controller.CP.radarUnavailable = False
|
||||
mock_CC = Mock()
|
||||
mock_CC.actuators = Mock(accel=1.0)
|
||||
mock_CC.longActive = True
|
||||
print("[a_value] starting accel_last:", self.controller.tuning.accel_last)
|
||||
# first pass: limit to jerk_upper * DT_CTRL * 2 = 0.1
|
||||
self.controller.jerk_upper = 0.1 / (DT_CTRL * 2)
|
||||
self.controller.accel_cmd = 1.0 # ensure accel_cmd is set
|
||||
self.controller.calculate_accel(mock_CC)
|
||||
print(f"[a_value] pass1 actual_accel={self.controller.actual_accel:.5f}")
|
||||
self.assertAlmostEqual(self.controller.actual_accel, 0.1, places=5)
|
||||
|
||||
# second pass: limit increment by new jerk_upper
|
||||
mock_CC.actuators.accel = 0.7
|
||||
self.controller.jerk_upper = 0.2 / (DT_CTRL * 2)
|
||||
self.controller.accel_cmd = 0.7 # update accel_cmd
|
||||
self.controller.calculate_accel(mock_CC)
|
||||
print(f"[a_value] pass2 actual_accel={self.controller.actual_accel:.5f}")
|
||||
self.assertAlmostEqual(self.controller.actual_accel, 0.3, places=5)
|
||||
|
||||
def test_make_jerk_realistic_profile(self):
|
||||
"""Test make_jerk with realistic velocity and acceleration profile"""
|
||||
np.random.seed(42)
|
||||
num_points = 30
|
||||
segments = [
|
||||
np.random.uniform(0.3, 0.8, num_points//4),
|
||||
np.random.uniform(0.8, 1.6, num_points//4),
|
||||
np.random.uniform(-0.2, 0.2, num_points//4),
|
||||
np.random.uniform(-1.2, -0.5, num_points//8),
|
||||
np.random.uniform(-2.2, -1.2, num_points//8)
|
||||
]
|
||||
accels = np.concatenate(segments)[:num_points]
|
||||
vels = np.zeros_like(accels)
|
||||
vels[0] = 5.0
|
||||
for i in range(1, len(accels)):
|
||||
vels[i] = max(0.0, min(30.0, vels[i-1] + accels[i-1] * (DT_CTRL*2)))
|
||||
mock_CC, mock_CS = Mock(), Mock()
|
||||
mock_CC.actuators, mock_CS.out = Mock(), Mock()
|
||||
mock_CC.longActive = True
|
||||
self.controller.stopping = False
|
||||
|
||||
# Test with LONG_TUNING_DYNAMIC only
|
||||
self.controller.CP_IQ.flags = HyundaiFlagsIQ.LONG_TUNING_DYNAMIC
|
||||
for v, a in zip(vels, accels, strict=True):
|
||||
mock_CS.out.vEgo = float(v)
|
||||
mock_CS.out.aEgo = float(a)
|
||||
mock_CS.aBasis = float(a)
|
||||
mock_CC.actuators.accel = float(a)
|
||||
self.controller.calculate_jerk(mock_CC, mock_CS, LongCtrlState.pid)
|
||||
print(f"[realistic][LONG_TUNING_DYNAMIC] v={v:.2f}, a={a:.2f}, jerk_upper={self.controller.jerk_upper:.2f}, jerk_lower={self.controller.jerk_lower:.2f}")
|
||||
self.assertGreater(self.controller.jerk_upper, 0.0)
|
||||
|
||||
# Reset controller before next test
|
||||
self.controller.tuning = LongitudinalState()
|
||||
self.controller.jerk_upper = 0.5
|
||||
self.controller.jerk_lower = 0.5
|
||||
|
||||
# Test with LONG_TUNING_DYNAMIC and LONG_TUNING_PREDICTIVE
|
||||
self.controller.CP_IQ.flags = HyundaiFlagsIQ.LONG_TUNING_DYNAMIC | HyundaiFlagsIQ.LONG_TUNING_PREDICTIVE
|
||||
for v, a in zip(vels, accels, strict=True):
|
||||
mock_CS.out.vEgo = float(v)
|
||||
mock_CS.out.aEgo = float(a)
|
||||
mock_CS.aBasis = float(a)
|
||||
mock_CC.actuators.accel = float(a)
|
||||
self.controller.calculate_jerk(mock_CC, mock_CS, LongCtrlState.pid)
|
||||
print(f"[realistic][LONG_TUNING_PREDICTIVE] v={v:.2f}, a={a:.2f}, " +
|
||||
f"jerk_upper={self.controller.jerk_upper:.2f}, jerk_lower={self.controller.jerk_lower:.2f}")
|
||||
self.assertGreater(self.controller.jerk_upper, 0.0)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
@@ -1,32 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
from enum import IntFlag
|
||||
|
||||
|
||||
class HyundaiSafetyFlagsIQ:
|
||||
DEFAULT = 0
|
||||
ESCC = 1
|
||||
LONG_MAIN_CRUISE_TOGGLEABLE = 2
|
||||
HAS_LDA_BUTTON = 4
|
||||
NON_SCC = 8
|
||||
|
||||
|
||||
class HyundaiFlagsIQ(IntFlag):
|
||||
"""
|
||||
Flags for Hyundai specific quirks within iqpilot.
|
||||
"""
|
||||
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
|
||||
|
||||
|
||||
@@ -8,12 +8,8 @@ from collections.abc import Callable
|
||||
|
||||
from iqdbc.car import structs
|
||||
from iqdbc.car.can_definitions import CanRecvCallable, CanSendCallable
|
||||
from iqdbc.car.hyundai.values import HyundaiFlags
|
||||
from iqdbc.car.subaru.values import SubaruFlags
|
||||
from iqdbc.lvbs.car.hyundai.enable_radar_tracks import enable_radar_tracks as hyundai_enable_radar_tracks
|
||||
from iqdbc.lvbs.car.hyundai.longitudinal.helpers import LongitudinalTuningType
|
||||
from iqdbc.lvbs.car.hyundai.values import HyundaiFlagsIQ
|
||||
from iqdbc.lvbs.car.subaru.values_ext import SubaruFlagsIQ, SubaruSafetyFlagsIQ
|
||||
from iqdbc.lvbs.car.subaru.iq_values import SubaruFlagsIQ, SubaruSafetyFlagsIQ
|
||||
from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ
|
||||
from iqdbc.lvbs.car.toyota.values import ToyotaFlagsIQ
|
||||
|
||||
@@ -81,7 +77,6 @@ def apply_iq_car_config(CI, CP: structs.CarParams, CP_IQ: structs.IQCarParams,
|
||||
|
||||
_apply_long_tuning(CI, CP, CP_IQ, params_dict)
|
||||
_apply_torque_blend(CP, CP_IQ, params_dict)
|
||||
_initialize_radar_tracks(CP, CP_IQ, can_recv, can_send)
|
||||
_apply_creep_assist(CP, CP_IQ, params_dict)
|
||||
_apply_toyota_options(CP, CP_IQ, params_dict)
|
||||
|
||||
@@ -89,14 +84,6 @@ def apply_iq_car_config(CI, CP: structs.CarParams, CP_IQ: structs.IQCarParams,
|
||||
def _apply_long_tuning(CI, CP: structs.CarParams, CP_IQ: structs.IQCarParams,
|
||||
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_IQ.flags |= HyundaiFlagsIQ.LONG_TUNING_DYNAMIC.value
|
||||
if hyundai_longitudinal_tuning == LongitudinalTuningType.PREDICTIVE:
|
||||
CP_IQ.flags |= HyundaiFlagsIQ.LONG_TUNING_PREDICTIVE.value
|
||||
|
||||
_ = CI.get_longitudinal_tuning_iq(CP, CP_IQ)
|
||||
|
||||
|
||||
@@ -108,25 +95,18 @@ def _apply_torque_blend(CP: structs.CarParams, CP_IQ: structs.IQCarParams,
|
||||
CP_IQ.flags |= TeslaFlagsIQ.COOP_STEERING.value
|
||||
|
||||
|
||||
def _initialize_radar_tracks(CP: structs.CarParams, CP_IQ: structs.IQCarParams,
|
||||
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_IQ.flags & HyundaiFlagsIQ.ENHANCED_SCC):
|
||||
tracks_enabled = hyundai_enable_radar_tracks(can_recv, can_send, bus=0, addr=0x7d0)
|
||||
CP.radarUnavailable = not tracks_enabled
|
||||
|
||||
|
||||
def _apply_creep_assist(CP: structs.CarParams, CP_IQ: structs.IQCarParams, 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("IQSubaruCreepAssist", 0)) == 1
|
||||
stop_and_go_manual_parking_brake = int(params_dict.get("IQSubaruCreepAssistManualBrake", 0)) == 1
|
||||
# Subaru stop-and-go; unsupported on gen2-global and hybrid platforms.
|
||||
if CP.brand != 'subaru' or CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID):
|
||||
return
|
||||
|
||||
if stop_and_go:
|
||||
CP_IQ.flags |= SubaruFlagsIQ.STOP_AND_GO.value
|
||||
if stop_and_go_manual_parking_brake:
|
||||
CP_IQ.flags |= SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE.value
|
||||
if stop_and_go or stop_and_go_manual_parking_brake:
|
||||
CP_IQ.safetyParam |= SubaruSafetyFlagsIQ.STOP_AND_GO
|
||||
if int(params_dict.get("IQSubaruCreepAssist", 0)) == 1:
|
||||
CP_IQ.flags |= SubaruFlagsIQ.STOP_AND_GO.value
|
||||
if int(params_dict.get("IQSubaruCreepAssistManualBrake", 0)) == 1:
|
||||
CP_IQ.flags |= SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE.value
|
||||
|
||||
if CP_IQ.flags & (SubaruFlagsIQ.STOP_AND_GO | SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE):
|
||||
CP_IQ.iqSafetyFlags |= SubaruSafetyFlagsIQ.STOP_AND_GO
|
||||
|
||||
|
||||
def _apply_toyota_options(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict: dict[str, str]) -> None:
|
||||
|
||||
@@ -1,3 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
@@ -1,3 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
@@ -1,7 +1,8 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
Nissan cruise-button reader: emits edge ButtonEvents for the RES/SET buttons.
|
||||
"""
|
||||
from enum import StrEnum
|
||||
|
||||
from iqdbc.car import Bus, structs
|
||||
@@ -9,24 +10,22 @@ from iqdbc.can.parser import CANParser
|
||||
from iqdbc.lvbs.car.nissan.values import BUTTONS
|
||||
|
||||
|
||||
class CarStateExt:
|
||||
class IQCarState:
|
||||
def __init__(self, CP, CP_IQ):
|
||||
self.CP = CP
|
||||
self.CP_IQ = CP_IQ
|
||||
|
||||
self.button_events = []
|
||||
self.button_events: list = []
|
||||
self.button_states = {button.event_type: False for button in BUTTONS}
|
||||
|
||||
def update(self, ret: structs.CarState, ret_iq: structs.IQCarState, can_parsers: dict[StrEnum, CANParser]):
|
||||
def update(self, ret: structs.CarState, ret_iq: structs.IQCarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
cp = can_parsers[Bus.pt]
|
||||
|
||||
button_events = []
|
||||
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:
|
||||
pressed = cp.vl[button.can_addr][button.can_msg] in button.values
|
||||
if pressed != self.button_states[button.event_type]:
|
||||
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
|
||||
event.pressed = pressed
|
||||
events.append(event)
|
||||
self.button_states[button.event_type] = pressed
|
||||
self.button_events = events
|
||||
@@ -1,8 +1,10 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
IQ.Pilot Nissan extension: safety flag + cruise-button table.
|
||||
"""
|
||||
from collections import namedtuple
|
||||
|
||||
from iqdbc.car import structs
|
||||
|
||||
|
||||
@@ -11,12 +13,9 @@ class NissanSafetyFlagsIQ:
|
||||
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]),
|
||||
|
||||
@@ -1,3 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
@@ -1,33 +1,30 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
Always-on-Lateral output adapter for Rivian: derives the per-frame lateral-active
|
||||
and lane-keep icon state the LKAS command needs from the shared AOL state.
|
||||
"""
|
||||
from collections import namedtuple
|
||||
|
||||
from iqdbc.car import structs
|
||||
from iqdbc.car.interfaces import CarStateBase
|
||||
|
||||
MAX_STEERING_ANGLE = 90.0
|
||||
# Rivian EPAS rejects large-angle torque, so lateral is dropped past this bound.
|
||||
_MAX_STEERING_ANGLE = 90.0
|
||||
|
||||
AolDataIQ = namedtuple("AolDataIQ",
|
||||
["lka_icon_states", "lat_active"])
|
||||
AolDataIQ = namedtuple("AolDataIQ", ["lka_icon_states", "lat_active"])
|
||||
|
||||
|
||||
class AolCarController:
|
||||
def __init__(self):
|
||||
self.aol = AolDataIQ(False, False)
|
||||
|
||||
self.lka_icon_states = False
|
||||
self.lat_active = False
|
||||
|
||||
def aol_status_update(self, CC: structs.CarControl, CC_IQ: structs.IQCarControl, CS: CarStateBase) -> AolDataIQ:
|
||||
if CC_IQ.aol.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 AolDataIQ(self.lka_icon_states, self.lat_active)
|
||||
|
||||
def update(self, CC: structs.CarControl, CC_IQ: structs.IQCarControl, CS: CarStateBase) -> None:
|
||||
self.aol = self.aol_status_update(CC, CC_IQ, CS)
|
||||
prev_active = self.aol.lat_active
|
||||
if CC_IQ.aol.available:
|
||||
lat_active = CC.latActive and abs(CS.out.steeringAngleDeg) < _MAX_STEERING_ANGLE
|
||||
lka_icon = prev_active
|
||||
else:
|
||||
lat_active = CC.latActive
|
||||
lka_icon = CC.enabled
|
||||
self.aol = AolDataIQ(lka_icon, lat_active)
|
||||
|
||||
@@ -1,5 +1,9 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
Rivian longitudinal-harness-upgrade carState reader: with the upgrade harness the
|
||||
right steering-wheel controls and the drive stalk drive the openpilot set speed,
|
||||
and the harness exposes blind-spot indicators. Only active behind the upgrade flag.
|
||||
"""
|
||||
import math
|
||||
from enum import StrEnum
|
||||
@@ -12,11 +16,13 @@ from iqdbc.lvbs.car.rivian.values import RivianFlagsIQ
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
|
||||
MAX_SET_SPEED = 85 * CV.MPH_TO_MS
|
||||
MIN_SET_SPEED = 20 * CV.MPH_TO_MS
|
||||
_SET_SPEED_MAX = 85 * CV.MPH_TO_MS
|
||||
_SET_SPEED_MIN = 20 * CV.MPH_TO_MS
|
||||
_LONG_PRESS_FRAMES = 66
|
||||
_STALK_HOLD_FRAMES = 50
|
||||
|
||||
|
||||
class CarStateExt:
|
||||
class IQCarState:
|
||||
def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams):
|
||||
self.CP = CP
|
||||
self.CP_IQ = CP_IQ
|
||||
@@ -29,57 +35,57 @@ class CarStateExt:
|
||||
self.decrease_counter = 0
|
||||
self.stalk_down_counter = 0
|
||||
|
||||
def _apply_set_speed_buttons(self, ret: structs.CarState, cp_park, cp_adas) -> None:
|
||||
was_increasing = self.increase_button
|
||||
was_decreasing = self.decrease_button
|
||||
|
||||
self.increase_button = cp_park.vl["WheelButtons"]["RightButton_RightClick"] == 2
|
||||
self.decrease_button = cp_park.vl["WheelButtons"]["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
|
||||
step = 10.0 if metric else 5.0
|
||||
shown = self.set_speed * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH)
|
||||
|
||||
# A held button steps to the next round multiple; a tap nudges by one unit.
|
||||
if self.increase_button:
|
||||
if self.increase_counter % _LONG_PRESS_FRAMES == 0:
|
||||
self.set_speed = math.ceil((shown + 1) / step) * step * conversion
|
||||
elif not was_increasing:
|
||||
self.set_speed += conversion
|
||||
if self.decrease_button:
|
||||
if self.decrease_counter % _LONG_PRESS_FRAMES == 0:
|
||||
self.set_speed = math.floor((shown - 1) / step) * step * conversion
|
||||
elif not was_decreasing:
|
||||
self.set_speed -= conversion
|
||||
|
||||
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"]["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"]["RightButton_RightClick"] == 2
|
||||
self.decrease_button = cp_park.vl["WheelButtons"]["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
|
||||
self._apply_set_speed_buttons(ret, cp_park, cp_adas)
|
||||
|
||||
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
|
||||
# Drive stalk held down (VDM_UserAdasRequest 3/4) for ~0.5s snaps set speed
|
||||
# up to the current speed, matching stock Rivian ACC (it never lowers it).
|
||||
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)
|
||||
if self.stalk_down_counter == _STALK_HOLD_FRAMES:
|
||||
self.set_speed = max(self.set_speed, ret.vEgoCluster)
|
||||
|
||||
self.set_speed = max(MIN_SET_SPEED, min(self.set_speed, MAX_SET_SPEED))
|
||||
self.set_speed = max(_SET_SPEED_MIN, min(self.set_speed, _SET_SPEED_MAX))
|
||||
ret.cruiseState.speed = self.set_speed
|
||||
|
||||
if self.CP.enableBsm:
|
||||
@@ -92,9 +98,7 @@ class CarStateExt:
|
||||
|
||||
@staticmethod
|
||||
def get_parser(CP, CP_IQ) -> dict[StrEnum, CANParser]:
|
||||
messages = {}
|
||||
|
||||
messages: dict[StrEnum, CANParser] = {}
|
||||
if CP_IQ.flags & RivianFlagsIQ.LONGITUDINAL_HARNESS_UPGRADE:
|
||||
messages[Bus.alt] = CANParser(DBC[CP.carFingerprint][Bus.alt], [], 5)
|
||||
|
||||
return messages
|
||||
@@ -1,10 +1,10 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
IQ.Pilot Rivian extension flags.
|
||||
"""
|
||||
from enum import IntFlag
|
||||
|
||||
|
||||
class RivianFlagsIQ(IntFlag):
|
||||
LONGITUDINAL_HARNESS_UPGRADE = 1
|
||||
|
||||
|
||||
|
||||
@@ -1,3 +0,0 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
@@ -1,45 +1,36 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
Always-on-Lateral input adapter for Subaru: turns the dashboard LKAS button into
|
||||
a carState lkas ButtonEvent so the shared AOL state machine can toggle lateral.
|
||||
"""
|
||||
|
||||
from enum import StrEnum
|
||||
from iqdbc.car import Bus, structs
|
||||
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.car.subaru.values import SubaruFlags
|
||||
from iqdbc.lvbs.aol_base import AolCarStateBase
|
||||
from iqdbc.can.parser import CANParser
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
_LKAS = structs.CarState.ButtonEvent.Type.lkas
|
||||
|
||||
# ES_LKAS_State/LKAS_Dash_State: 0 = neutral, 1 = LKAS shown on, 2 = LKAS shown off
|
||||
_DASH_ON = 1
|
||||
_DASH_OFF = 2
|
||||
|
||||
|
||||
class AolCarState(AolCarStateBase):
|
||||
def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams):
|
||||
super().__init__(CP, CP_IQ)
|
||||
|
||||
@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_aol(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
if self.CP.flags & SubaruFlags.PREGLOBAL:
|
||||
return
|
||||
|
||||
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"]
|
||||
self.lkas_button = can_parsers[Bus.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})
|
||||
if self._is_toggle_edge():
|
||||
ret.buttonEvents = [*ret.buttonEvents, structs.CarState.ButtonEvent(type=_LKAS, pressed=True)]
|
||||
|
||||
def _is_toggle_edge(self) -> bool:
|
||||
# every dash-state change is a deliberate press except the off->on rebound (2 -> 1)
|
||||
if self.lkas_button == self.prev_lkas_button:
|
||||
return False
|
||||
return not (self.prev_lkas_button == _DASH_OFF and self.lkas_button == _DASH_ON)
|
||||
|
||||
@@ -1,7 +1,11 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
IQ.Pilot Subaru stop-and-go: nudges the ACC out of a standstill it would
|
||||
otherwise hold. Two variants gated by user flags — an electronic-parking-brake
|
||||
resume pulse (distance/lead triggered) and a manual-parking-brake hold-timer
|
||||
resume. Both work by spoofing the camera-bus throttle/brake frames.
|
||||
"""
|
||||
import copy
|
||||
from enum import StrEnum
|
||||
|
||||
@@ -10,97 +14,68 @@ from iqdbc.car.can_definitions import CanData
|
||||
from iqdbc.car.interfaces import CarStateBase
|
||||
from iqdbc.car.subaru.values import SubaruFlags
|
||||
|
||||
from iqdbc.lvbs.car.subaru import subarucan_ext
|
||||
from iqdbc.lvbs.car.subaru.values_ext import SubaruFlagsIQ
|
||||
from iqdbc.lvbs.car.subaru import iq_subarucan
|
||||
from iqdbc.lvbs.car.subaru.iq_values import SubaruFlagsIQ
|
||||
from iqdbc.can.parser import CANParser
|
||||
|
||||
_SNG_ACC_MIN_DIST = 3
|
||||
_SNG_ACC_MAX_DIST = 4.5
|
||||
# EPB resume fires only while the lead is pulling away within this gap band (m).
|
||||
_RESUME_GAP_MIN = 3.0
|
||||
_RESUME_GAP_MAX = 4.5
|
||||
_EPB_PULSE_FRAMES = 15
|
||||
|
||||
|
||||
class IQStopAndGoController:
|
||||
def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams):
|
||||
self.CP = CP
|
||||
self.CP_IQ = CP_IQ
|
||||
self.enabled = CP_IQ.flags & (SubaruFlagsIQ.STOP_AND_GO | SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE)
|
||||
self.manual_parking_brake = CP_IQ.flags & SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE
|
||||
self.enabled = bool(CP_IQ.flags & (SubaruFlagsIQ.STOP_AND_GO | SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE))
|
||||
self.manual_parking_brake = bool(CP_IQ.flags & SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE)
|
||||
|
||||
self.last_standstill_frame = 0
|
||||
self.epb_resume_frames_remaining = -1
|
||||
self.prev_close_distance = 0.0
|
||||
self.standstill_since = 0
|
||||
self._pulse_left = 0
|
||||
self.prev_gap = 0.0
|
||||
|
||||
def update_epb_resume_sequence(self, should_resume: bool) -> bool:
|
||||
def _epb_pulse(self, trigger: bool) -> bool:
|
||||
# A trigger arms a fixed-length resume pulse; the pulse then plays out frame by frame.
|
||||
if self.manual_parking_brake:
|
||||
return False
|
||||
if trigger:
|
||||
self._pulse_left = _EPB_PULSE_FRAMES
|
||||
if self._pulse_left > 0:
|
||||
self._pulse_left -= 1
|
||||
return True
|
||||
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
|
||||
"""
|
||||
|
||||
def _want_resume(self, CC: structs.CarControl, CS: CarStateBase, frame: int) -> bool:
|
||||
if not CC.enabled or not CC.hudControl.leadVisible:
|
||||
return False
|
||||
|
||||
close_distance = CS.es_distance_msg["Close_Distance"]
|
||||
in_standstill = CS.out.standstill
|
||||
gap = CS.es_distance_msg["Close_Distance"]
|
||||
standing = CS.out.standstill
|
||||
if not standing:
|
||||
self.standstill_since = frame
|
||||
|
||||
if not in_standstill:
|
||||
self.last_standstill_frame = frame
|
||||
hold_arm, hold_reset = (0.75, 0.8) if self.CP.flags & SubaruFlags.PREGLOBAL else (0.5, 0.55)
|
||||
held_for = (frame - self.standstill_since) * DT_CTRL
|
||||
held_long_enough = held_for > hold_arm
|
||||
if held_for >= hold_reset:
|
||||
self.standstill_since = 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
|
||||
lead_pulling_away = _RESUME_GAP_MIN < gap < _RESUME_GAP_MAX and gap > self.prev_gap
|
||||
self.prev_gap = gap
|
||||
|
||||
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
|
||||
return held_long_enough
|
||||
return self._epb_pulse(standing and lead_pulling_away)
|
||||
|
||||
def create_creep_assist(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))
|
||||
return []
|
||||
|
||||
resume = self._want_resume(CC, CS, frame)
|
||||
can_sends = [iq_subarucan.create_throttle(packer, self.CP, CS.throttle_msg, 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))
|
||||
|
||||
can_sends.append(iq_subarucan.create_brake_pedal(packer, self.CP, CS.brake_pedal_msg, resume and self.manual_parking_brake))
|
||||
return can_sends
|
||||
|
||||
|
||||
@@ -108,14 +83,11 @@ class IQStopAndGoState:
|
||||
def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams):
|
||||
self.CP = CP
|
||||
self.CP_IQ = CP_IQ
|
||||
|
||||
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"])
|
||||
|
||||
41
iqdbc_repo/iqdbc/lvbs/car/subaru/iq_subarucan.py
Normal file
41
iqdbc_repo/iqdbc/lvbs/car/subaru/iq_subarucan.py
Normal file
@@ -0,0 +1,41 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
Camera-bus message spoofers for Subaru stop-and-go: re-emit the stock Throttle
|
||||
and Brake_Pedal frames, nudging a single field to trigger an ACC resume from a
|
||||
standstill. Field sets mirror the DBC message layout.
|
||||
"""
|
||||
from iqdbc.car.subaru.values import CanBus, SubaruFlags
|
||||
|
||||
_THROTTLE_FIELDS_PREGLOBAL = ("Throttle_Pedal", "Signal1", "Not_Full_Throttle", "Signal2", "Engine_RPM",
|
||||
"Off_Throttle", "Signal3", "Throttle_Cruise", "Throttle_Combo", "Throttle_Body",
|
||||
"Off_Throttle_2", "Signal4")
|
||||
_THROTTLE_FIELDS_GLOBAL = ("CHECKSUM", "Signal1", "Engine_RPM", "Neutral", "Throttle_Pedal", "Throttle_Cruise",
|
||||
"Throttle_Combo", "Signal3", "Off_Accel")
|
||||
_BRAKE_FIELDS_PREGLOBAL = ("Speed", "Brake_Pedal", "Signal1")
|
||||
_BRAKE_FIELDS_GLOBAL = ("CHECKSUM", "Signal1", "Speed", "Signal2", "Brake_Lights", "Signal3", "Brake_Pedal", "Signal4")
|
||||
|
||||
|
||||
def _next_counter(msg):
|
||||
return (msg["COUNTER"] + 1) % 0x10
|
||||
|
||||
|
||||
def create_throttle(packer, CP, throttle_msg, send_resume):
|
||||
preglobal = bool(CP.flags & SubaruFlags.PREGLOBAL)
|
||||
fields = _THROTTLE_FIELDS_PREGLOBAL if preglobal else _THROTTLE_FIELDS_GLOBAL
|
||||
values = {name: throttle_msg[name] for name in fields}
|
||||
values["COUNTER"] = _next_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):
|
||||
preglobal = bool(CP.flags & SubaruFlags.PREGLOBAL)
|
||||
fields = _BRAKE_FIELDS_PREGLOBAL if preglobal else _BRAKE_FIELDS_GLOBAL
|
||||
values = {name: brake_pedal_msg[name] for name in fields}
|
||||
if not preglobal:
|
||||
values["COUNTER"] = _next_counter(brake_pedal_msg)
|
||||
if send_resume:
|
||||
values["Speed"] = 1 if preglobal else 3
|
||||
return packer.make_can_msg("Brake_Pedal", CanBus.camera, values)
|
||||
@@ -1,14 +1,16 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
IQ.Pilot Subaru extension flags. Selected in apply_iq_car_config() from the user's
|
||||
stop-and-go params and consumed by the stop-and-go controller and panda safety.
|
||||
"""
|
||||
|
||||
from enum import IntFlag
|
||||
|
||||
|
||||
class SubaruSafetyFlagsIQ:
|
||||
STOP_AND_GO = 1
|
||||
|
||||
|
||||
class SubaruFlagsIQ(IntFlag):
|
||||
STOP_AND_GO = 1
|
||||
STOP_AND_GO_MANUAL_PARKING_BRAKE = 2
|
||||
|
||||
|
||||
class SubaruSafetyFlagsIQ:
|
||||
STOP_AND_GO = 1
|
||||
@@ -1,68 +0,0 @@
|
||||
from iqdbc.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)
|
||||
@@ -12,15 +12,24 @@ from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class CarStateExt:
|
||||
class IQCarState:
|
||||
def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams):
|
||||
self.CP = CP
|
||||
self.CP_IQ = CP_IQ
|
||||
|
||||
self.infotainment_3_finger_press = 0
|
||||
self.vehicle_bus_available = bool(CP_IQ.flags & TeslaFlagsIQ.HAS_VEHICLE_BUS)
|
||||
|
||||
def update(self, ret: structs.CarState, ret_iq: structs.IQCarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
if self.CP_IQ.flags & TeslaFlagsIQ.HAS_VEHICLE_BUS:
|
||||
if Bus.adas in can_parsers:
|
||||
cp_adas = can_parsers[Bus.adas]
|
||||
|
||||
odometer_km = float(cp_adas.vl["ID3B6UI_odometer"].get("UI_odometer", 0.0))
|
||||
if 0.0 < odometer_km < 4294967.296:
|
||||
self.vehicle_bus_available = True
|
||||
ret.odometer = odometer_km
|
||||
|
||||
if self.vehicle_bus_available and Bus.adas in can_parsers:
|
||||
cp_adas = can_parsers[Bus.adas]
|
||||
|
||||
prev_infotainment_3_finger_press = self.infotainment_3_finger_press
|
||||
@@ -66,7 +75,9 @@ class CarStateExt:
|
||||
def get_parser(CP: structs.CarParams, CP_IQ: structs.IQCarParams) -> dict[StrEnum, CANParser]:
|
||||
messages = {}
|
||||
|
||||
if CP_IQ.flags & TeslaFlagsIQ.HAS_VEHICLE_BUS:
|
||||
# Only tap the vehicle bus on cars where fingerprinting saw it: an always-on
|
||||
# bus-1 parser trips bus_timeout -> canBusMissing on harnesses without the tap.
|
||||
if CP_IQ.flags & TeslaFlagsIQ.HAS_VEHICLE_BUS and Bus.adas in DBC[CP.carFingerprint]:
|
||||
messages[Bus.adas] = CANParser(DBC[CP.carFingerprint][Bus.adas], [], CANBUS.vehicle)
|
||||
|
||||
return messages
|
||||
@@ -1,12 +1,25 @@
|
||||
import json
|
||||
import os
|
||||
|
||||
from iqdbc.lvbs.car.car_catalog import build_car_catalog, CAR_LIST_JSON_OUT
|
||||
from iqdbc.car.common.basedir import BASEDIR
|
||||
from iqdbc.lvbs.car.car_catalog import build_car_catalog
|
||||
|
||||
CATALOG_JSON = os.path.join(BASEDIR, "..", "..", "iqpilot", "selfdrive", "car", "vehicle_catalog.json")
|
||||
|
||||
_KEY_TO_ATTR = {"id": "platform", "mk": "make", "grp": "brand", "mdl": "model", "yrs": "year", "req": "package"}
|
||||
|
||||
|
||||
def _decode(envelope) -> dict:
|
||||
out = {}
|
||||
for record in (envelope.get("vehicles") or {}).values():
|
||||
out[record.get("label", "")] = {attr: record.get(key) for key, attr in _KEY_TO_ATTR.items()}
|
||||
return out
|
||||
|
||||
|
||||
class TestCarList:
|
||||
def test_generator(self):
|
||||
generated_car_list = json.dumps(build_car_catalog(), indent=2, ensure_ascii=False)
|
||||
with open(CAR_LIST_JSON_OUT) as f:
|
||||
current_car_list = f.read()
|
||||
generated = build_car_catalog()
|
||||
with open(CATALOG_JSON) as f:
|
||||
shipped = _decode(json.load(f))
|
||||
|
||||
assert generated_car_list == current_car_list, "Run iqdbc/lvbs/car/car_catalog.py to update the car list"
|
||||
assert shipped == generated, "Run: python -m openpilot.iqpilot.selfdrive.car.vehicle_catalog"
|
||||
|
||||
@@ -21,7 +21,7 @@ ZSS_DIFF_THRESHOLD = 4
|
||||
ZSS_MAX_THRESHOLD = 10
|
||||
|
||||
|
||||
class CarStateExt:
|
||||
class IQCarState:
|
||||
def __init__(self, CP, CP_IQ):
|
||||
self.CP = CP
|
||||
self.CP_IQ = CP_IQ
|
||||
Reference in New Issue
Block a user