97 lines
3.0 KiB
Python
97 lines
3.0 KiB
Python
from collections import deque
|
|
import copy
|
|
import math
|
|
|
|
from cereal import car
|
|
from openpilot.common.conversions import Conversions as CV
|
|
from opendbc.can.parser import CANParser
|
|
from opendbc.can.can_define import CANDefine
|
|
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
|
|
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_GEARS, CAMERA_SCC_CAR, \
|
|
CANFD_CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams
|
|
from openpilot.selfdrive.car.interfaces import CarStateBase
|
|
from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController
|
|
|
|
from openpilot.common.watcher import Watcher
|
|
|
|
PREV_BUTTON_SAMPLES = 8
|
|
CLUSTER_SAMPLE_RATE = 20 # frames
|
|
STANDSTILL_THRESHOLD = 12 * 0.03125 * CV.KPH_TO_MS
|
|
|
|
|
|
class CarState(CarStateBase):
|
|
def __init__(self, CP):
|
|
super().__init__(CP)
|
|
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
|
|
|
self.cruise_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
|
|
self.main_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
|
|
|
|
self.params = CarControllerParams(CP)
|
|
|
|
def update(self, cp, cp_cam):
|
|
Watcher.log_watch("hyundai_carstate_update_cp", cp)
|
|
Watcher.log_watch("hyundai_carstate_update_cp_cam", cp_cam)
|
|
|
|
if self.CP.carFingerprint in CANFD_CAR:
|
|
return self.update_canfd(cp, cp_cam)
|
|
|
|
Watcher.log_watch("hyundai_carstate_update_self", self)
|
|
return ret
|
|
|
|
def update_canfd(self, cp, cp_cam):
|
|
Watcher.log_watch("hyundai_carstate_update_canfd_cp", cp)
|
|
Watcher.log_watch("hyundai_carstate_update_canfd_cp_cam", cp_cam)
|
|
return ret
|
|
|
|
def get_can_parser(self, CP):
|
|
messages = [
|
|
(self.gear_msg_canfd, 100),
|
|
(self.accelerator_msg_canfd, 100),
|
|
("WHEEL_SPEEDS", 100),
|
|
("STEERING_SENSORS", 100),
|
|
("MDPS", 100),
|
|
("TCS", 50),
|
|
("CRUISE_BUTTONS_ALT", 50),
|
|
("BLINKERS", 4),
|
|
("DOORS_SEATBELTS", 4),
|
|
]
|
|
|
|
if CP.carFingerprint in EV_CAR:
|
|
messages += [
|
|
("MANUAL_SPEED_LIMIT_ASSIST", 10),
|
|
]
|
|
|
|
if not (CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS):
|
|
messages += [
|
|
("CRUISE_BUTTONS", 50)
|
|
]
|
|
|
|
if CP.enableBsm:
|
|
messages += [
|
|
("BLINDSPOTS_REAR_CORNERS", 20),
|
|
]
|
|
|
|
# if CP.nav_msg:
|
|
# messages.append(("CLUSTER_SPEED_LIMIT", 10))
|
|
|
|
if not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and not CP.openpilotLongitudinalControl:
|
|
messages += [
|
|
("SCC_CONTROL", 50),
|
|
]
|
|
|
|
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).ECAN)
|
|
|
|
@staticmethod
|
|
def get_cam_can_parser(CP):
|
|
messages = []
|
|
if CP.flags & HyundaiFlags.CANFD_HDA2:
|
|
block_lfa_msg = "CAM_0x362" if CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else "CAM_0x2a4"
|
|
messages += [(block_lfa_msg, 20)]
|
|
elif CP.flags & HyundaiFlags.CANFD_CAMERA_SCC:
|
|
messages += [
|
|
("SCC_CONTROL", 50),
|
|
]
|
|
|
|
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).CAM)
|