brianhansonxyz/oscarpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Your Name
2024-03-09 10:33:28 -06:00
parent cd3bd61918
commit f6831761bf
16 changed files with 5259 additions and 643 deletions
Download Patch File Download Diff File Expand all files Collapse all files

332
selfdrive/car/hyundai/carcontroller.2 Normal file
View File

@@ -0,0 +1,332 @@
from cereal import car
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import clip
from openpilot.common.realtime import DT_CTRL
from opendbc.can.packer import CANPacker
from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance
from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR, LEGACY_SAFETY_MODE_CAR
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
from openpilot.common.params import Params
from openpilot.common.watcher import Watcher
VisualAlert = car.CarControl.HUDControl.VisualAlert
LongCtrlState = car.CarControl.Actuators.LongControlState
params_memory = Params("/dev/shm/params")
# EPS faults if you apply torque while the steering angle is above 90 degrees for more than 1 second
# All slightly below EPS thresholds to avoid fault
MAX_ANGLE = 85
MAX_ANGLE_FRAMES = 89
MAX_ANGLE_CONSECUTIVE_FRAMES = 2
def process_hud_alert(enabled, fingerprint, hud_control):
sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
# initialize to no line visible
# TODO: this is not accurate for all cars
sys_state = 1
if hud_control.leftLaneVisible and hud_control.rightLaneVisible or sys_warning: # HUD alert only display when LKAS status is active
sys_state = 3 if enabled or sys_warning else 4
elif hud_control.leftLaneVisible:
sys_state = 5
elif hud_control.rightLaneVisible:
sys_state = 6
# initialize to no warnings
left_lane_warning = 0
right_lane_warning = 0
if hud_control.leftLaneDepart:
left_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2
if hud_control.rightLaneDepart:
right_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2
return sys_warning, sys_state, left_lane_warning, right_lane_warning
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.CAN = CanBus(CP)
self.params = CarControllerParams(CP)
self.packer = CANPacker(dbc_name)
self.angle_limit_counter = 0
self.frame = 0
self.accel_last = 0
self.apply_steer_last = 0
self.car_fingerprint = CP.carFingerprint
self.last_button_frame = 0
self.last_resume_frame = 0
self.last_debug_frame = 0
def update(self, CC, CS, now_nanos, sport_plus):
actuators = CC.actuators
hud_control = CC.hudControl
# hud_v_cruise = hud_control.setSpeed
# if hud_v_cruise > 70:
# hud_v_cruise = 0
# steering torque
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
# >90 degree steering fault prevention
self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringAngleDeg) >= MAX_ANGLE, CC.latActive,
self.angle_limit_counter, MAX_ANGLE_FRAMES,
MAX_ANGLE_CONSECUTIVE_FRAMES)
if not CC.latActive:
apply_steer = 0
# Hold torque with induced temporary fault when cutting the actuation bit
torque_fault = CC.latActive and not apply_steer_req
self.apply_steer_last = apply_steer
# accel + longitudinal
if sport_plus:
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX_PLUS)
else:
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
stopping = actuators.longControlState == LongCtrlState.stopping
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
# HUD messages
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
hud_control)
can_sends = []
# *** common hyundai stuff ***
# tester present - w/ no response (keeps relevant ECU disabled)
if self.frame % 100 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and self.CP.openpilotLongitudinalControl:
# for longitudinal control, either radar or ADAS driving ECU
addr, bus = 0x7d0, 0
if self.CP.flags & HyundaiFlags.CANFD_HDA2.value:
addr, bus = 0x730, self.CAN.ECAN
can_sends.append([addr, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", bus])
# for blinkers
if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", self.CAN.ECAN])
# CAN-FD platforms
if self.CP.carFingerprint in CANFD_CAR:
hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
hda2_long = hda2 and self.CP.openpilotLongitudinalControl
# steering control
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer))
# MODIFIED BBOT
if self.frame % 5 == 0 and hda2:
can_sends.append(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS.hda2_lfa_block_msg,
self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING))
# MODIFIED BBOT
# LFA and HDA icons
if self.frame % 5 == 0 and (not hda2 or hda2_long):
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.CAN, self.frame, CC.leftBlinker, CC.rightBlinker))
# can_sends.append(hyundaicanfd.create_misc_messages(self.packer, self.CAN, self.frame))
if self.CP.openpilotLongitudinalControl:
if hda2:
can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
if self.frame % 2 == 0:
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
set_speed_in_units, CS.personality_profile))
self.accel_last = accel
else:
# button presses
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False, set_speed_in_units = None))
else:
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, apply_steer_req,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
left_lane_warning, right_lane_warning))
if not self.CP.openpilotLongitudinalControl:
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True))
if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
# TODO: unclear if this is needed
jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
hud_control.leadVisible, set_speed_in_units, stopping,
CC.cruiseControl.override, use_fca, CS.out.cruiseState.available, CS.personality_profile))
# 20 Hz LFA MFA message
if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled))
# 5 Hz ACC options
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.extend(hyundaican.create_acc_opt(self.packer))
# 2 Hz front radar options
if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.append(hyundaican.create_frt_radar_opt(self.packer))
# CSLC
# if not self.CP.openpilotLongitudinalControl and CS.cruiseState.available and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# if False and not self.CP.openpilotLongitudinalControl and CC.enabled and CC.experimental_mode and CS.cruiseState.available and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# # cslcSetSpeed = get_set_speed(self, hud_v_cruise)
# cslcSetSpeed = set_speed_in_units
# self.cruise_button = get_cslc_button(self, cslcSetSpeed, CS)
# if self.cruise_button != Buttons.NONE:
# # if self.CP.carFingerprint in LEGACY_SAFETY_MODE_CAR:
# # send_freq = 1
# # # send resume at a max freq of 10Hz
# # if (self.frame - self.last_button_frame) * DT_CTRL > 0.1 * send_freq:
# # # send 25 messages at a time to increases the likelihood of cruise buttons being accepted
# # can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
# # if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15 * send_freq:
# # self.last_button_frame = self.frame
# if self.frame % 2 == 0:
# if self.CP.carFingerprint in CANFD_CAR:
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, self.cruise_button))
# else:
# can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
# Test - Works???
# if CS.cruise_buttons == Buttons.NONE and CS.cruiseState.enabled:
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, Buttons.SET_DECEL))
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / self.params.STEER_MAX
new_actuators.steerOutputCan = apply_steer
new_actuators.accel = accel
Watcher.log_watch("hyundai_carcontroller_update_CC", CC)
Watcher.log_watch("hyundai_carcontroller_update_CS", CS)
Watcher.log_watch("hyundai_carcontroller_update_self", self)
self.frame += 1
return new_actuators, can_sends
def create_button_messages(self, CC: car.CarControl, CS: car.CarState, use_clu11: bool, set_speed_in_units = None):
can_sends = []
# Test me.
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, 1, Buttons.RES_ACCEL))
# if self.CP.openpilotLongitudinalControl:
# CC.cruiseControl.resume = True
# self.CP.openpilotLongitudinalControl = False
# else:
# CC.cruiseControl.cancel = True
# self.CP.openpilotLongitudinalControl = True
if use_clu11:
if CC.cruiseControl.cancel:
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP.carFingerprint))
elif CC.cruiseControl.resume:
# send resume at a max freq of 10Hz
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
# send 25 messages at a time to increases the likeli M,hood of resume being accepted
can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL, self.CP.carFingerprint)] * 25)
if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15:
self.last_button_frame = self.frame
else:
if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
# if (self.frame - self.last_button_frame) * DT_CTRL > 3:
# if self.last_resume_frame = self.frame
if CS.oscar_lane_center_btn_pressed:
CS.oscar_lane_center_btn_pressed = False
# CC.cruiseControl.resume = True
CC.cruiseControl.cancel = True
# Test this...
# Also try create_acc_commands
# This attempts to set the speed to
# stopping = CC.actuators.longControlState == LongCtrlState.stopping
# can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, CC.actuators.accel, stopping, CC.cruiseControl.override,
# 40, CS.personality_profile))
# cruise cancel
if CC.cruiseControl.cancel:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.cruise_info))
self.last_button_frame = self.frame
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
self.last_button_frame = self.frame
# cruise standstill resume
elif CC.cruiseControl.resume:
if (self.frame - self.last_button_frame) * DT_CTRL > 4:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
# for _ in range(20):
nothing = 0
# Nothing for now --?
# oscar - test me
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL))
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL))
self.last_button_frame = self.frame
self.last_resume_frame = self.frame
elif set_speed_in_units is not None and not self.CP.openpilotLongitudinalControl and CS.cruiseState.enabled and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# if False and not self.CP.openpilotLongitudinalControl and CC.enabled and CC.experimental_mode and CS.cruiseState.available and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# # cslcSetSpeed = get_set_speed(self, hud_v_cruise)
cslcSetSpeed = set_speed_in_units
self.cruise_button = get_cslc_button(self, cslcSetSpeed, CS)
if self.cruise_button != Buttons.NONE:
# if self.CP.carFingerprint in LEGACY_SAFETY_MODE_CAR:
# send_freq = 1
# # send resume at a max freq of 10Hz
# if (self.frame - self.last_button_frame) * DT_CTRL > 0.1 * send_freq:
# # send 25 messages at a time to increases the likelihood of cruise buttons being accepted
# can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
# if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15 * send_freq:
# self.last_button_frame = self.frame
if self.frame % 2 == 0:
for _ in range(20):
if self.CP.carFingerprint in CANFD_CAR:
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, self.cruise_button))
else:
can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
self.last_button_frame = self.frame
# can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.SET_DECEL, self.CP.carFingerprint)] * 25)
return can_sends
def get_set_speed(self, hud_v_cruise):
v_cruise_kph = min(hud_v_cruise * CV.MS_TO_KPH, V_CRUISE_MAX)
v_cruise = int(round(v_cruise_kph * CV.KPH_TO_MPH))
v_cruise_slc: int = 0
# v_cruise_slc = params_memory.get_int("CSLCSpeed")
if v_cruise_slc > 0:
v_cruise = v_cruise_slc
return v_cruise
def get_cslc_button(self, cslcSetSpeed, CS, CC):
cruiseBtn = Buttons.NONE
speedSetPoint = int(round(CS.out.cruiseState.speed * CV.MS_TO_MPH))
if cslcSetSpeed < speedSetPoint and speedSetPoint > 25 and CC.enabled and CC.experimental_mode:
cruiseBtn = Buttons.SET_DECEL
elif cslcSetSpeed > speedSetPoint and speedSetPoint < 85 and CC.enabled:
cruiseBtn = Buttons.RES_ACCEL
else:
cruiseBtn = Buttons.SET_DECEL
# cruiseBtn = Buttons.NONE
return cruiseBtn

512
selfdrive/car/hyundai/carcontroller.py
View File

@@ -12,6 +12,18 @@ from openpilot.common.params import Params
from openpilot.common.watcher import Watcher
# DEVELOPOMENT TODO
# - Write a loop that runs and does nothing.
# - Load as many inputs in possible into state variables.
# - Make a state variable tester. It should be a web server running async.
# - Make a button tester, also on web server, to test engaging each button.
# - Write basic interface refactor
# - Write experimental mode speed controller
# - Write resume from stop controller
# - Implement as many extra features as possible (auto hvac, auto sunroof, suggest a break, remind headlights)
VisualAlert = car.CarControl.HUDControl.VisualAlert
LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -23,7 +35,148 @@ MAX_ANGLE = 85
MAX_ANGLE_FRAMES = 89
MAX_ANGLE_CONSECUTIVE_FRAMES = 2
# Constants for state arrays.
ENGAGED = 'engaged'
STANDBY = 'standby'
OFF = 'off'
# Input
# Current state of openpilot and its desired commands
openpilot_state = {
'openpilot_ready': False, # Openpilot is enabled in settings
'lateral_active': False, # Lateral control is engaged either full or always on lateral
'openpilot_engaged': False, # Openpilot is full engaged (lateral + cruise control)
'always_on_lateral_active': False, # Cruise control disengaged but lateral still engaged
'experimental_active': False, # Frogpilot has engaged experimental control, so we need to adjust speed and possibly apply brakes (if this is possible)
'experimental_desired_speed': 0, # The speed experimental control wants to set long to
'curviture_angle': 0, # Detected current curviture (ideally within the next 3 or 4 seconds) for taking over LKAS and possibly LONG
'lane_change_enabled': False, # The lane change assist feature is active
'lane_change_active': False, # Executing a lane change assist
'connected_to_internet': False,
'rate_limited_internet': False
}
# Input
# Esoteric things related to the car that support other features than driving. Will be shown in debugger.
car_state = {
'location_lat': 0,
'location_long': 0,
'drive_distance_this_trip': 0,
'drive_distance_today': 0,
'compass_direction': 'N', # Unclear if I will use this
'fuel': 1, # 0-1 fuel tank level
'windows_down': False, # True if any windows / sunroof is open
'climate_control': False,
'climate_control_set_temp': 72,
'cabin_temprature': 50,
'heated_seat_driver_on': False,
'heated_seat_passenger_on': False,
'heated_steering_wheel_on': False,
'fan_seat_driver_on': False,
'fan_seat_passenger_on': False,
'info_panel_item_showing': 0, # If we can capture what instrument panel info is showing, we can change it to speed
'daytime': False,
'headlights': False, # Would be cool to remind to have the headlights turned on if moving
}
# Input
# The current status of cruise control activation
cruise_control_state = {
'set_speed': 0, # Cruise control set speed
'actual_speed': 0, # Spedometer reading
'speed_limit': 0, # Speed limit as reported by car's dashboard
'cruise_control_status': OFF, # or ENGAGED or STANDBY
'brake_pressed': False
}
# Input
# The current state of the car in front of us, if cruise control is engaged but the car is not moving
stationary_state = {
'stationary': False, # When speed reaches 0 we are stationary
'stationary_since': 0, # At what time stationary was achieved
'stationary_has_lead': False, # At the time of becoming stationary, was there a lead veichele?
'stationary_has_lead_distance': 0, # At the time of becoming stationary, what was the distance to the lead veichele?
'current_lead_car_distance': 0, # What is the distance to the lead veichele now?
'lead_vehicle_moving_away': False, # Has logic decided that the current veichele is moving away?
'oem_lanes_detected': False # Is the car reporting it can see lanes? I would like to experiment with engaging stock LKAS.
}
# Input
# What buttons the user is pressing. Can be None, BUTTON_SHORT, or BUTTON_LONG (held for 1/2 second)
cruise_control_buttons_input = {
'engage_cruise_control': None,
'cruise_control_speed_up': None,
'cruise_control_speed_down': None,
'pause_resume_cruise_control': None,
'lane_keep_assist_button': None,
'lane_follow_assist_button': None
}
# Output
# What buttons we wish to simulate pressing.
cruise_control_buttons_output = {
'engage_cruise_control': False,
'cruise_control_speed_up': False,
'cruise_control_speed_down': False,
'pause_resume_cruise_control': False,
'lane_keep_assist_button': False,
'lane_follow_assist_button': False,
}
# Output
# What buttons we wish to simulate pressing.
# I don't know if I will have access to these.
other_buttons_output = {
'info_category': False,
'info_section': False,
'heated_seat_driver': False,
'heated_seat_passenger': False,
'heated_steering_wheel': False,
'fan_seat_driver': False,
'fan_seat_passenger': False,
'sunroof': False,
'hvac_power': False,
'hvac_sync': False,
'hvac_up': False,
'hvac_down': False
}
# What to display on the instrument panel. Sent every frame.
instrument_panel_outputs = {
'lane_keeping_assist_active': False,
'forward_collision_warning_active': False,
'high_beam_assist_active': False,
'forward_collision_avoidance_assist_active': False,
'blind_spot_collision_warning_active': False,
'lane_following_assist_active': False,
'driver_attention_warning_active': False,
'lane_departure_warning_active': False
}
# State
# Variables to save what oscarpilot is doing - behaviors will change depending
# on current state
oscarpilot_state = [
'set_speed': 0, # Current desired cruise speed
'speed_override': False, # Set if moving away from set speed due to experimental
'speed_restore': False, # Set if moving twoards set speed due to reengagement or change speed to speed limit
'lkas_oem': False, # Set if allowing the car oem software to control LKAS
'daytime': False, # Set if it is daytime. Car is more judgmental at night.
]
# Output
# These are messages to the user, and get set to false when processed by openpilot
# Ideally we can set these in the infotainment area as well, like carplay audio selection
oscarpilot_alerts = {
'detected_stop_sign_or_light': False, # Openpilot has seen a stoplight or stopsign
'very_sharp_curve': False, # The upcoming curve is probably too sharp to be handled automatically
'lane_confidence_low': False, # The confidence level that openpilot knows what its doing is low
'heavy_traffic_ahead': False, # Mapping data indicates heavy traffic within 2 miles
'weather_ahead': False, # Mapping data indicates precipitation within 5 miles
'suggest_a_break': False, # Variable will be used to suggest a break (3 hours at day, 1.5 hours at night)
}
# Where is this called? Public? Private? Should we refactor?
def process_hud_alert(enabled, fingerprint, hud_control):
sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
@@ -47,7 +200,6 @@ def process_hud_alert(enabled, fingerprint, hud_control):
return sys_warning, sys_state, left_lane_warning, right_lane_warning
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
@@ -64,28 +216,90 @@ class CarController:
self.last_resume_frame = 0
self.last_debug_frame = 0
self._CC = None
# self._CC.actuators
# self._CC.hudControl
self._CS = None
self._now_nanos = None
self._sport_plus = None
self._actuators = None
self._hud_control = None
def update(self, CC, CS, now_nanos, sport_plus):
actuators = CC.actuators
hud_control = CC.hudControl
self._CC = CC
self._CS = CS
self._now_nanos = now_nanos
self._sport_plus = sport_plus
# hud_v_cruise = hud_control.setSpeed
# if hud_v_cruise > 70:
# hud_v_cruise = 0
self._apply_steer = None
self._accel = None
can_sends = []
# CAN-FD car logic
if self.CP.carFingerprint in CANFD_CAR:
# Steering control for CAN FD
# can_sends.extend(self._create_common_messages())
can_sends.extend(self._create_steering_messages())
can_sends.extend(self._create_instrument_messages())
can_sends.extend(self._create_button_messages())
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / self.params.STEER_MAX
new_actuators.steerOutputCan = apply_steer
new_actuators.accel = accel
self.frame += 1
return new_actuators, can_sends
# def _create_common_messages():
# *** common hyundai stuff ***
def _unprocessed():
# HUD messages
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
hud_control)
can_sends = []
# CAN-FD platforms
# if self.CP.carFingerprint in CANFD_CAR:
# # LFA and HDA icons
# if self.frame % 5 == 0 and (not hda2 or hda2_long):
# can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled))
# if self.CP.openpilotLongitudinalControl:
# if hda2:
# can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
# if self.frame % 2 == 0:
# can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
# set_speed_in_units, CS.personality_profile))
# self.accel_last = accel
# else:
# # button presses
# can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False))
# Mostly unchanged from frogpilot.
def _create_steering_messages():
can_sends = []
# steering torque
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, self._CS.out.steeringTorque, self.params)
# >90 degree steering fault prevention
self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringAngleDeg) >= MAX_ANGLE, CC.latActive,
self.angle_limit_counter, apply_steer_req = common_fault_avoidance(abs(self._CS.out.steeringAngleDeg) >= MAX_ANGLE, self._CC.latActive,
self.angle_limit_counter, MAX_ANGLE_FRAMES,
MAX_ANGLE_CONSECUTIVE_FRAMES)
if not CC.latActive:
apply_steer = 0
# Hold torque with induced temporary fault when cutting the actuation bit
torque_fault = CC.latActive and not apply_steer_req
torque_fault = self._CC.latActive and not apply_steer_req
self.apply_steer_last = apply_steer
@@ -94,240 +308,70 @@ class CarController:
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX_PLUS)
else:
accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
stopping = actuators.longControlState == LongCtrlState.stopping
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH)
# HUD messages
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
hud_control)
hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
can_sends = []
# steering control
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer))
# *** common hyundai stuff ***
# tester present - w/ no response (keeps relevant ECU disabled)
if self.frame % 100 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and self.CP.openpilotLongitudinalControl:
# for longitudinal control, either radar or ADAS driving ECU
addr, bus = 0x7d0, 0
if self.CP.flags & HyundaiFlags.CANFD_HDA2.value:
addr, bus = 0x730, self.CAN.ECAN
can_sends.append([addr, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", bus])
# for blinkers
if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", self.CAN.ECAN])
# CAN-FD platforms
if self.CP.carFingerprint in CANFD_CAR:
hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2
hda2_long = hda2 and self.CP.openpilotLongitudinalControl
# steering control
can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer))
# MODIFIED BBOT
if self.frame % 5 == 0 and hda2:
can_sends.append(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS.hda2_lfa_block_msg,
# prevent LFA from activating on HDA2 by sending "no lane lines detected" to ADAS ECU
if self.frame % 5 == 0 and hda2:
can_sends.append(hyundaicanfd.create_suppress_lfa(self.packer, self.CAN, CS.hda2_lfa_block_msg,
self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING))
# MODIFIED BBOT
# LFA and HDA icons
if self.frame % 5 == 0 and (not hda2 or hda2_long):
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.CAN, self.frame, CC.leftBlinker, CC.rightBlinker))
# can_sends.append(hyundaicanfd.create_misc_messages(self.packer, self.CAN, self.frame))
if self.CP.openpilotLongitudinalControl:
if hda2:
can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame))
if self.frame % 2 == 0:
can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override,
set_speed_in_units, CS.personality_profile))
self.accel_last = accel
else:
# button presses
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False, set_speed_in_units = None))
else:
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, apply_steer_req,
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
left_lane_warning, right_lane_warning))
if not self.CP.openpilotLongitudinalControl:
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True))
if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
# TODO: unclear if this is needed
jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
hud_control.leadVisible, set_speed_in_units, stopping,
CC.cruiseControl.override, use_fca, CS.out.cruiseState.available, CS.personality_profile))
# 20 Hz LFA MFA message
if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled))
# 5 Hz ACC options
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.extend(hyundaican.create_acc_opt(self.packer))
# 2 Hz front radar options
if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
can_sends.append(hyundaican.create_frt_radar_opt(self.packer))
# CSLC
# if not self.CP.openpilotLongitudinalControl and CS.cruiseState.available and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# if False and not self.CP.openpilotLongitudinalControl and CC.enabled and CC.experimental_mode and CS.cruiseState.available and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# # cslcSetSpeed = get_set_speed(self, hud_v_cruise)
# cslcSetSpeed = set_speed_in_units
# self.cruise_button = get_cslc_button(self, cslcSetSpeed, CS)
# if self.cruise_button != Buttons.NONE:
# # if self.CP.carFingerprint in LEGACY_SAFETY_MODE_CAR:
# # send_freq = 1
# # # send resume at a max freq of 10Hz
# # if (self.frame - self.last_button_frame) * DT_CTRL > 0.1 * send_freq:
# # # send 25 messages at a time to increases the likelihood of cruise buttons being accepted
# # can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
# # if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15 * send_freq:
# # self.last_button_frame = self.frame
# if self.frame % 2 == 0:
# if self.CP.carFingerprint in CANFD_CAR:
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, self.cruise_button))
# else:
# can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
# Test - Works???
# if CS.cruise_buttons == Buttons.NONE and CS.cruiseState.enabled:
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, Buttons.SET_DECEL))
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / self.params.STEER_MAX
new_actuators.steerOutputCan = apply_steer
new_actuators.accel = accel
Watcher.log_watch("hyundai_carcontroller_update_CC", CC)
Watcher.log_watch("hyundai_carcontroller_update_CS", CS)
Watcher.log_watch("hyundai_carcontroller_update_self", self)
self.frame += 1
return new_actuators, can_sends
def create_button_messages(self, CC: car.CarControl, CS: car.CarState, use_clu11: bool, set_speed_in_units = None):
can_sends = []
# Test me.
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, 1, Buttons.RES_ACCEL))
# if self.CP.openpilotLongitudinalControl:
# CC.cruiseControl.resume = True
# self.CP.openpilotLongitudinalControl = False
# else:
# CC.cruiseControl.cancel = True
# self.CP.openpilotLongitudinalControl = True
if use_clu11:
if CC.cruiseControl.cancel:
can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP.carFingerprint))
elif CC.cruiseControl.resume:
# send resume at a max freq of 10Hz
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1:
# send 25 messages at a time to increases the likeli M,hood of resume being accepted
can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL, self.CP.carFingerprint)] * 25)
if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15:
self.last_button_frame = self.frame
else:
if (self.frame - self.last_button_frame) * DT_CTRL > 0.25:
# if (self.frame - self.last_button_frame) * DT_CTRL > 3:
# if self.last_resume_frame = self.frame
if CS.oscar_lane_center_btn_pressed:
CS.oscar_lane_center_btn_pressed = False
# CC.cruiseControl.resume = True
CC.cruiseControl.cancel = True
# Test this...
# Also try create_acc_commands
# This attempts to set the speed to
# stopping = CC.actuators.longControlState == LongCtrlState.stopping
# can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, CC.actuators.accel, stopping, CC.cruiseControl.override,
# 40, CS.personality_profile))
# cruise cancel
if CC.cruiseControl.cancel:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
can_sends.append(hyundaicanfd.create_acc_cancel(self.packer, self.CP, self.CAN, CS.cruise_info))
self.last_button_frame = self.frame
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
self.last_button_frame = self.frame
# cruise standstill resume
elif CC.cruiseControl.resume:
if (self.frame - self.last_button_frame) * DT_CTRL > 4:
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
# for _ in range(20):
nothing = 0
# Nothing for now --?
# oscar - test me
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
# can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL))
else:
for _ in range(20):
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL))
self.last_button_frame = self.frame
self.last_resume_frame = self.frame
elif set_speed_in_units is not None and not self.CP.openpilotLongitudinalControl and CS.cruiseState.enabled and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# if False and not self.CP.openpilotLongitudinalControl and CC.enabled and CC.experimental_mode and CS.cruiseState.available and not CS.out.gasPressed and CS.cruise_buttons == Buttons.NONE:
# # cslcSetSpeed = get_set_speed(self, hud_v_cruise)
cslcSetSpeed = set_speed_in_units
self.cruise_button = get_cslc_button(self, cslcSetSpeed, CS)
if self.cruise_button != Buttons.NONE:
# if self.CP.carFingerprint in LEGACY_SAFETY_MODE_CAR:
# send_freq = 1
# # send resume at a max freq of 10Hz
# if (self.frame - self.last_button_frame) * DT_CTRL > 0.1 * send_freq:
# # send 25 messages at a time to increases the likelihood of cruise buttons being accepted
# can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
# if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15 * send_freq:
# self.last_button_frame = self.frame
if self.frame % 2 == 0:
for _ in range(20):
if self.CP.carFingerprint in CANFD_CAR:
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, self.cruise_button))
else:
can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP.carFingerprint)] * 25)
self.last_button_frame = self.frame
# can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.SET_DECEL, self.CP.carFingerprint)] * 25)
# blinkers
# if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
# can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.CAN, self.frame, CC.leftBlinker, CC.rightBlinker))
self._apply_steer = apply_steer
self._accel = accel
return can_sends
# Placeholder for potential instrument panel processing
def _create_instrument_messages(instrument_panel_state):
can_sends = []
# Assume default values or read the initial state
lfa_icon_state = 0
hda_active = 0
hda_icon_state = 0
hda_set_speed = 0 # This would likely need to be more dynamic based on context
# Assign values based on instrument_panel_outputs states
for output in instrument_panel_outputs:
if output['name'] == 'lane_keeping_assist_active':
lfa_icon_state = 2 if output['value'] else 0
elif output['name'] == 'hda_active': # Hypothetical key for example
hda_active = 1 if output['value'] else 0
elif output['name'] == 'hda_icon_state': # Also hypothetical
hda_icon_state = 2 if output['value'] else 0
# Add other conditions based on the actual data structure you expect
# Assume we can bundle all these states into one message as in your example
values = {
"LFA_Icon_State": lfa_icon_state,
"HDA_Active": hda_active,
"HDA_Icon_State": hda_icon_state,
"HDA_VSetReq": hda_set_speed, # This example assumes static speed; adjust if dynamic
}
can_sends.append(packer.make_can_msg("LFAHDA_MFC", 0, values))
def get_set_speed(self, hud_v_cruise):
v_cruise_kph = min(hud_v_cruise * CV.MS_TO_KPH, V_CRUISE_MAX)
v_cruise = int(round(v_cruise_kph * CV.KPH_TO_MPH))
return can_sends
v_cruise_slc: int = 0
# v_cruise_slc = params_memory.get_int("CSLCSpeed")
# Placeholder for potential button press processing
def _create_button_messages(button_presses):
can_sends = []
if v_cruise_slc > 0:
v_cruise = v_cruise_slc
return v_cruise
for button in cruise_control_buttons:
if button['value']:
# Resetting button state to prevent repeated presses
button['value'] = False
def get_cslc_button(self, cslcSetSpeed, CS, CC):
cruiseBtn = Buttons.NONE
speedSetPoint = int(round(CS.out.cruiseState.speed * CV.MS_TO_MPH))
if button['name'] in button_map:
# Mapping each button to a specific CAN message
btn_value = button_map[button['name']]
can_sends.append(create_buttons(packer, CP, CAN, 0, btn_value)) # Counter is example, replace with actual logic if necessary
if cslcSetSpeed < speedSetPoint and speedSetPoint > 25 and CC.enabled and CC.experimental_mode:
cruiseBtn = Buttons.SET_DECEL
elif cslcSetSpeed > speedSetPoint and speedSetPoint < 85 and CC.enabled:
cruiseBtn = Buttons.RES_ACCEL
else:
cruiseBtn = Buttons.SET_DECEL
# cruiseBtn = Buttons.NONE
return cruiseBtn
return can_sends

500
selfdrive/car/hyundai/carstate.org Normal file
View File

@@ -0,0 +1,500 @@
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.gear_msg_canfd = "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \
"GEAR_ALT_2" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS_2 else \
"GEAR_SHIFTER"
if CP.carFingerprint in CANFD_CAR:
self.shifter_values = can_define.dv[self.gear_msg_canfd]["GEAR"]
elif self.CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"]
elif self.CP.carFingerprint in CAN_GEARS["use_tcu_gears"]:
self.shifter_values = can_define.dv["TCU12"]["CUR_GR"]
else: # preferred and elect gear methods use same definition
self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
self.accelerator_msg_canfd = "ACCELERATOR" if CP.carFingerprint in EV_CAR else \
"ACCELERATOR_ALT" if CP.carFingerprint in HYBRID_CAR else \
"ACCELERATOR_BRAKE_ALT"
self.cruise_btns_msg_canfd = "CRUISE_BUTTONS_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else \
"CRUISE_BUTTONS"
self.is_metric = False
self.buttons_counter = 0
self.cruise_info = {}
# On some cars, CLU15->CF_Clu_VehicleSpeed can oscillate faster than the dash updates. Sample at 5 Hz
self.cluster_speed = 0
self.cluster_speed_counter = CLUSTER_SAMPLE_RATE
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)
ret = car.CarState.new_message()
cp_cruise = cp_cam if self.CP.carFingerprint in CAMERA_SCC_CAR else cp
self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0
speed_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
ret.doorOpen = any([cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"],
cp.vl["CGW2"]["CF_Gway_RLDrSw"], cp.vl["CGW2"]["CF_Gway_RRDrSw"]])
ret.seatbeltUnlatched = cp.vl["CGW1"]["CF_Gway_DrvSeatBeltSw"] == 0
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHL_SPD11"]["WHL_SPD_FL"],
cp.vl["WHL_SPD11"]["WHL_SPD_FR"],
cp.vl["WHL_SPD11"]["WHL_SPD_RL"],
cp.vl["WHL_SPD11"]["WHL_SPD_RR"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
self.cluster_speed_counter += 1
if self.cluster_speed_counter > CLUSTER_SAMPLE_RATE:
self.cluster_speed = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"]
self.cluster_speed_counter = 0
# Mimic how dash converts to imperial.
# Sorento is the only platform where CF_Clu_VehicleSpeed is already imperial when not is_metric
# TODO: CGW_USM1->CF_Gway_DrLockSoundRValue may describe this
if not self.is_metric and self.CP.carFingerprint not in (CAR.KIA_SORENTO,):
self.cluster_speed = math.floor(self.cluster_speed * CV.KPH_TO_MPH + CV.KPH_TO_MPH)
ret.vEgoCluster = self.cluster_speed * speed_conv
ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"]
ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"]
ret.yawRate = cp.vl["ESP12"]["YAW_RATE"]
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(
50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"], cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"]
ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
# cruise state
if self.CP.openpilotLongitudinalControl:
# These are not used for engage/disengage since openpilot keeps track of state using the buttons
ret.cruiseState.available = self.main_enabled
ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
ret.cruiseState.standstill = False
ret.cruiseState.nonAdaptive = False
else:
ret.cruiseState.available = cp_cruise.vl["SCC11"]["MainMode_ACC"] == 1
ret.cruiseState.enabled = cp_cruise.vl["SCC12"]["ACCMode"] != 0
ret.cruiseState.standstill = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 4.
ret.cruiseState.nonAdaptive = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 2. # Shows 'Cruise Control' on dash
ret.cruiseState.speed = cp_cruise.vl["SCC11"]["VSetDis"] * speed_conv
# TODO: Find brake pressure
ret.brake = 0
ret.brakePressed = cp.vl["TCS13"]["DriverOverride"] == 2 # 2 includes regen braking by user on HEV/EV
ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
ret.accFaulted = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR):
if self.CP.carFingerprint in HYBRID_CAR:
ret.gas = cp.vl["E_EMS11"]["CR_Vcu_AccPedDep_Pos"] / 254.
else:
ret.gas = cp.vl["E_EMS11"]["Accel_Pedal_Pos"] / 254.
ret.gasPressed = ret.gas > 0
else:
ret.gas = cp.vl["EMS12"]["PV_AV_CAN"] / 100.
ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"])
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection,
# as this seems to be standard over all cars, but is not the preferred method.
if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR):
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
elif self.CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
gear = cp.vl["CLU15"]["CF_Clu_Gear"]
elif self.CP.carFingerprint in CAN_GEARS["use_tcu_gears"]:
gear = cp.vl["TCU12"]["CUR_GR"]
else:
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
if not self.CP.openpilotLongitudinalControl:
aeb_src = "FCA11" if self.CP.flags & HyundaiFlags.USE_FCA.value else "SCC12"
aeb_sig = "FCA_CmdAct" if self.CP.flags & HyundaiFlags.USE_FCA.value else "AEB_CmdAct"
aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0
scc_warning = cp_cruise.vl["SCC12"]["TakeOverReq"] == 1 # sometimes only SCC system shows an FCW
aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src][aeb_sig] != 0
ret.stockFcw = (aeb_warning or scc_warning) and not aeb_braking
ret.stockAeb = aeb_warning and aeb_braking
if self.CP.enableBsm:
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0
# save the entire LKAS11 and CLU11
self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
self.clu11 = copy.copy(cp.vl["CLU11"])
self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE
self.prev_cruise_buttons = self.cruise_buttons[-1]
self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"])
self.prev_main_buttons = self.main_buttons[-1]
self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"])
if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
self.main_enabled = not self.main_enabled
# BBot functions for lfa and gap buttons - test speed up / down
self.oscar_lane_center_btn_pressed= False
self.oscar_slc_decel = False
self.oscar_slc_accel = False
self.param_memory.put("oscar_debug", "Hello World")
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.oscar_lane_center_btn_pressed= True
# lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"]
# if lkas_pressed and not self.lkas_previously_pressed:
# self.custom_speed_up = True
# self.lkas_previously_pressed = lkas_pressed
# Driving personalities function
# if self.personalities_via_wheel and ret.cruiseState.available:
# # Sync with the onroad UI button
# if self.param_memory.get_bool("PersonalityChangedViaUI"):
# self.personality_profile = self.param.get_int("LongitudinalPersonality")
# self.param_memory.put_bool("PersonalityChangedViaUI", False)
# # Change personality upon steering wheel button press
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.param_memory.put_bool("PersonalityChangedViaWheel", True)
# self.personality_profile = (self.previous_personality_profile + 2) % 3
# if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0:
# self.param.put_int("LongitudinalPersonality", self.personality_profile)
# self.previous_personality_profile = self.personality_profile
# # Toggle Experimental Mode from steering wheel function
# if self.experimental_mode_via_lkas and ret.cruiseState.available:
# lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"]
# if lkas_pressed and not self.lkas_previously_pressed:
# if self.conditional_experimental_mode:
# # Set "CEStatus" to work with "Conditional Experimental Mode"
# conditional_status = self.param_memory.get_int("CEStatus")
# override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2
# self.param_memory.put_int("CEStatus", override_value)
# else:
# experimental_mode = self.param.get_bool("ExperimentalMode")
# # Invert the value of "ExperimentalMode"
# put_bool_nonblocking("ExperimentalMode", not experimental_mode)
# self.lkas_previously_pressed = lkas_pressed
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)
ret = car.CarState.new_message()
self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["DISTANCE_UNIT"] != 1
speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
if self.CP.carFingerprint in (EV_CAR | HYBRID_CAR):
offset = 255. if self.CP.carFingerprint in EV_CAR else 1023.
ret.gas = cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL"] / offset
ret.gasPressed = ret.gas > 1e-5
else:
ret.gasPressed = bool(cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL_PRESSED"])
ret.brakePressed = cp.vl["TCS"]["DriverBraking"] == 1
ret.doorOpen = cp.vl["DOORS_SEATBELTS"]["DRIVER_DOOR"] == 1
ret.seatbeltUnlatched = cp.vl["DOORS_SEATBELTS"]["DRIVER_SEATBELT"] == 0
gear = cp.vl[self.gear_msg_canfd]["GEAR"]
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
# TODO: figure out positions
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_1"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_2"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_3"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_4"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"]
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"]
ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0
# TODO: alt signal usage may be described by cp.vl['BLINKERS']['USE_ALT_LAMP']
left_blinker_sig, right_blinker_sig = "LEFT_LAMP", "RIGHT_LAMP"
if self.CP.carFingerprint == CAR.KONA_EV_2ND_GEN:
left_blinker_sig, right_blinker_sig = "LEFT_LAMP_ALT", "RIGHT_LAMP_ALT"
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"][left_blinker_sig],
cp.vl["BLINKERS"][right_blinker_sig])
if self.CP.enableBsm:
ret.leftBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FL_INDICATOR"] != 0
ret.rightBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FR_INDICATOR"] != 0
# cruise state
# CAN FD cars enable on main button press, set available if no TCS faults preventing engagement
ret.cruiseState.available = self.main_enabled
if self.CP.openpilotLongitudinalControl:
# These are not used for engage/disengage since openpilot keeps track of state using the buttons
ret.cruiseState.enabled = cp.vl["TCS"]["ACC_REQ"] == 1
ret.cruiseState.standstill = False
else:
cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp
ret.cruiseState.enabled = cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] in (1, 2)
ret.cruiseState.standstill = cp_cruise_info.vl["SCC_CONTROL"]["CRUISE_STANDSTILL"] == 1
ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor
self.cruise_info = copy.copy(cp_cruise_info.vl["SCC_CONTROL"])
# Manual Speed Limit Assist is a feature that replaces non-adaptive cruise control on EV CAN FD platforms.
# It limits the vehicle speed, overridable by pressing the accelerator past a certain point.
# The car will brake, but does not respect positive acceleration commands in this mode
# TODO: find this message on ICE & HYBRID cars + cruise control signals (if exists)
if self.CP.carFingerprint in EV_CAR:
ret.cruiseState.nonAdaptive = cp.vl["MANUAL_SPEED_LIMIT_ASSIST"]["MSLA_ENABLED"] == 1
self.prev_cruise_buttons = self.cruise_buttons[-1]
self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"])
self.prev_main_buttons = self.main_buttons[-1]
self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"])
if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
self.main_enabled = not self.main_enabled
self.buttons_counter = cp.vl[self.cruise_btns_msg_canfd]["COUNTER"]
ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
if self.CP.flags & HyundaiFlags.CANFD_HDA2:
self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING
else cp_cam.vl["CAM_0x2a4"])
SpeedLimitController.load_state()
SpeedLimitController.car_speed_limit = self.calculate_speed_limit_canfd(self.CP, cp, cp_cam) * speed_factor
SpeedLimitController.write_car_state()
# self.custom_speed_up = False
self.oscar_lane_center_btn_pressed = False
if ret.cruiseState.available:
if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
self.oscar_lane_center_btn_pressed = True
lkas_pressed = False
try:
lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"]
except:
nothing = 0
# intentionally cause a failure
if lkas_pressed:
floog=norpdywoop
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.oscar_lane_center_btn_pressed= True
# Driving personalities function
# if self.personalities_via_wheel and ret.cruiseState.available:
# # Sync with the onroad UI button
# if self.param_memory.get_bool("PersonalityChangedViaUI"):
# self.personality_profile = self.param.get_int("LongitudinalPersonality")
# self.param_memory.put_bool("PersonalityChangedViaUI", False)
# # Change personality upon steering wheel button press
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.param_memory.put_bool("PersonalityChangedViaWheel", True)
# self.personality_profile = (self.previous_personality_profile + 2) % 3
# if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0:
# self.param.put_int("LongitudinalPersonality", self.personality_profile)
# self.previous_personality_profile = self.personality_profile
# # Toggle Experimental Mode from steering wheel function
# if self.experimental_mode_via_lkas and ret.cruiseState.available:
# lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"]
# if lkas_pressed and not self.lkas_previously_pressed:
# if self.conditional_experimental_mode:
# # Set "CEStatus" to work with "Conditional Experimental Mode"
# conditional_status = self.param_memory.get_int("CEStatus")
# override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2
# self.param_memory.put_int("CEStatus", override_value)
# else:
# experimental_mode = self.param.get_bool("ExperimentalMode")
# # Invert the value of "ExperimentalMode"
# put_bool_nonblocking("ExperimentalMode", not experimental_mode)
# self.lkas_previously_pressed = lkas_pressed
Watcher.log_watch("hyundai_carstate_update_canfd_self", self)
return ret
# BBOT does not work
def calculate_speed_limit_canfd(self, CP, cp, cp_cam):
try:
self._speed_limit_clu = cp.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"]
return self._speed_limit_clu if self._speed_limit_clu not in (0, 255) else 0
except:
return 0
def get_can_parser(self, CP):
if CP.carFingerprint in CANFD_CAR:
return self.get_can_parser_canfd(CP)
messages = [
# address, frequency
("MDPS12", 50),
("TCS13", 50),
("TCS15", 10),
("CLU11", 50),
("CLU15", 5),
("ESP12", 100),
("CGW1", 10),
("CGW2", 5),
("CGW4", 5),
("WHL_SPD11", 50),
("SAS11", 100),
]
if not CP.openpilotLongitudinalControl and CP.carFingerprint not in CAMERA_SCC_CAR:
messages += [
("SCC11", 50),
("SCC12", 50),
]
if CP.flags & HyundaiFlags.USE_FCA.value:
messages.append(("FCA11", 50))
if CP.enableBsm:
messages.append(("LCA11", 50))
if CP.carFingerprint in (HYBRID_CAR | EV_CAR):
messages.append(("E_EMS11", 50))
else:
messages += [
("EMS12", 100),
("EMS16", 100),
]
if CP.carFingerprint in (HYBRID_CAR | EV_CAR):
messages.append(("ELECT_GEAR", 20))
elif CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
pass
elif CP.carFingerprint in CAN_GEARS["use_tcu_gears"]:
messages.append(("TCU12", 100))
else:
messages.append(("LVR12", 100))
messages.append(("BCM_PO_11", 50))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
@staticmethod
def get_cam_can_parser(CP):
if CP.carFingerprint in CANFD_CAR:
return CarState.get_cam_can_parser_canfd(CP)
messages = [
("LKAS11", 100)
]
if not CP.openpilotLongitudinalControl and CP.carFingerprint in CAMERA_SCC_CAR:
messages += [
("SCC11", 50),
("SCC12", 50),
]
if CP.flags & HyundaiFlags.USE_FCA.value:
messages.append(("FCA11", 50))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
def get_can_parser_canfd(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_canfd(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)

408
selfdrive/car/hyundai/carstate.py
View File

@@ -27,32 +27,6 @@ class CarState(CarStateBase):
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.gear_msg_canfd = "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \
"GEAR_ALT_2" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS_2 else \
"GEAR_SHIFTER"
if CP.carFingerprint in CANFD_CAR:
self.shifter_values = can_define.dv[self.gear_msg_canfd]["GEAR"]
elif self.CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
self.shifter_values = can_define.dv["CLU15"]["CF_Clu_Gear"]
elif self.CP.carFingerprint in CAN_GEARS["use_tcu_gears"]:
self.shifter_values = can_define.dv["TCU12"]["CUR_GR"]
else: # preferred and elect gear methods use same definition
self.shifter_values = can_define.dv["LVR12"]["CF_Lvr_Gear"]
self.accelerator_msg_canfd = "ACCELERATOR" if CP.carFingerprint in EV_CAR else \
"ACCELERATOR_ALT" if CP.carFingerprint in HYBRID_CAR else \
"ACCELERATOR_BRAKE_ALT"
self.cruise_btns_msg_canfd = "CRUISE_BUTTONS_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else \
"CRUISE_BUTTONS"
self.is_metric = False
self.buttons_counter = 0
self.cruise_info = {}
# On some cars, CLU15->CF_Clu_VehicleSpeed can oscillate faster than the dash updates. Sample at 5 Hz
self.cluster_speed = 0
self.cluster_speed_counter = CLUSTER_SAMPLE_RATE
self.params = CarControllerParams(CP)
def update(self, cp, cp_cam):
@@ -62,391 +36,15 @@ class CarState(CarStateBase):
if self.CP.carFingerprint in CANFD_CAR:
return self.update_canfd(cp, cp_cam)
ret = car.CarState.new_message()
cp_cruise = cp_cam if self.CP.carFingerprint in CAMERA_SCC_CAR else cp
self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0
speed_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
ret.doorOpen = any([cp.vl["CGW1"]["CF_Gway_DrvDrSw"], cp.vl["CGW1"]["CF_Gway_AstDrSw"],
cp.vl["CGW2"]["CF_Gway_RLDrSw"], cp.vl["CGW2"]["CF_Gway_RRDrSw"]])
ret.seatbeltUnlatched = cp.vl["CGW1"]["CF_Gway_DrvSeatBeltSw"] == 0
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHL_SPD11"]["WHL_SPD_FL"],
cp.vl["WHL_SPD11"]["WHL_SPD_FR"],
cp.vl["WHL_SPD11"]["WHL_SPD_RL"],
cp.vl["WHL_SPD11"]["WHL_SPD_RR"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
self.cluster_speed_counter += 1
if self.cluster_speed_counter > CLUSTER_SAMPLE_RATE:
self.cluster_speed = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"]
self.cluster_speed_counter = 0
# Mimic how dash converts to imperial.
# Sorento is the only platform where CF_Clu_VehicleSpeed is already imperial when not is_metric
# TODO: CGW_USM1->CF_Gway_DrLockSoundRValue may describe this
if not self.is_metric and self.CP.carFingerprint not in (CAR.KIA_SORENTO,):
self.cluster_speed = math.floor(self.cluster_speed * CV.KPH_TO_MPH + CV.KPH_TO_MPH)
ret.vEgoCluster = self.cluster_speed * speed_conv
ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"]
ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"]
ret.yawRate = cp.vl["ESP12"]["YAW_RATE"]
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(
50, cp.vl["CGW1"]["CF_Gway_TurnSigLh"], cp.vl["CGW1"]["CF_Gway_TurnSigRh"])
ret.steeringTorque = cp.vl["MDPS12"]["CR_Mdps_StrColTq"]
ret.steeringTorqueEps = cp.vl["MDPS12"]["CR_Mdps_OutTq"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
ret.steerFaultTemporary = cp.vl["MDPS12"]["CF_Mdps_ToiUnavail"] != 0 or cp.vl["MDPS12"]["CF_Mdps_ToiFlt"] != 0
# cruise state
if self.CP.openpilotLongitudinalControl:
# These are not used for engage/disengage since openpilot keeps track of state using the buttons
ret.cruiseState.available = self.main_enabled
ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
ret.cruiseState.standstill = False
ret.cruiseState.nonAdaptive = False
else:
ret.cruiseState.available = cp_cruise.vl["SCC11"]["MainMode_ACC"] == 1
ret.cruiseState.enabled = cp_cruise.vl["SCC12"]["ACCMode"] != 0
ret.cruiseState.standstill = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 4.
ret.cruiseState.nonAdaptive = cp_cruise.vl["SCC11"]["SCCInfoDisplay"] == 2. # Shows 'Cruise Control' on dash
ret.cruiseState.speed = cp_cruise.vl["SCC11"]["VSetDis"] * speed_conv
# TODO: Find brake pressure
ret.brake = 0
ret.brakePressed = cp.vl["TCS13"]["DriverOverride"] == 2 # 2 includes regen braking by user on HEV/EV
ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
ret.accFaulted = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR):
if self.CP.carFingerprint in HYBRID_CAR:
ret.gas = cp.vl["E_EMS11"]["CR_Vcu_AccPedDep_Pos"] / 254.
else:
ret.gas = cp.vl["E_EMS11"]["Accel_Pedal_Pos"] / 254.
ret.gasPressed = ret.gas > 0
else:
ret.gas = cp.vl["EMS12"]["PV_AV_CAN"] / 100.
ret.gasPressed = bool(cp.vl["EMS16"]["CF_Ems_AclAct"])
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection,
# as this seems to be standard over all cars, but is not the preferred method.
if self.CP.carFingerprint in (HYBRID_CAR | EV_CAR):
gear = cp.vl["ELECT_GEAR"]["Elect_Gear_Shifter"]
elif self.CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
gear = cp.vl["CLU15"]["CF_Clu_Gear"]
elif self.CP.carFingerprint in CAN_GEARS["use_tcu_gears"]:
gear = cp.vl["TCU12"]["CUR_GR"]
else:
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
if not self.CP.openpilotLongitudinalControl:
aeb_src = "FCA11" if self.CP.flags & HyundaiFlags.USE_FCA.value else "SCC12"
aeb_sig = "FCA_CmdAct" if self.CP.flags & HyundaiFlags.USE_FCA.value else "AEB_CmdAct"
aeb_warning = cp_cruise.vl[aeb_src]["CF_VSM_Warn"] != 0
scc_warning = cp_cruise.vl["SCC12"]["TakeOverReq"] == 1 # sometimes only SCC system shows an FCW
aeb_braking = cp_cruise.vl[aeb_src]["CF_VSM_DecCmdAct"] != 0 or cp_cruise.vl[aeb_src][aeb_sig] != 0
ret.stockFcw = (aeb_warning or scc_warning) and not aeb_braking
ret.stockAeb = aeb_warning and aeb_braking
if self.CP.enableBsm:
ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
ret.rightBlindspot = cp.vl["LCA11"]["CF_Lca_IndRight"] != 0
# save the entire LKAS11 and CLU11
self.lkas11 = copy.copy(cp_cam.vl["LKAS11"])
self.clu11 = copy.copy(cp.vl["CLU11"])
self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"] # 0 NOT ACTIVE, 1 ACTIVE
self.prev_cruise_buttons = self.cruise_buttons[-1]
self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"])
self.prev_main_buttons = self.main_buttons[-1]
self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"])
if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
self.main_enabled = not self.main_enabled
# BBot functions for lfa and gap buttons - test speed up / down
self.oscar_lane_center_btn_pressed= False
self.oscar_slc_decel = False
self.oscar_slc_accel = False
self.param_memory.put("oscar_debug", "Hello World")
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.oscar_lane_center_btn_pressed= True
# lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"]
# if lkas_pressed and not self.lkas_previously_pressed:
# self.custom_speed_up = True
# self.lkas_previously_pressed = lkas_pressed
# Driving personalities function
# if self.personalities_via_wheel and ret.cruiseState.available:
# # Sync with the onroad UI button
# if self.param_memory.get_bool("PersonalityChangedViaUI"):
# self.personality_profile = self.param.get_int("LongitudinalPersonality")
# self.param_memory.put_bool("PersonalityChangedViaUI", False)
# # Change personality upon steering wheel button press
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.param_memory.put_bool("PersonalityChangedViaWheel", True)
# self.personality_profile = (self.previous_personality_profile + 2) % 3
# if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0:
# self.param.put_int("LongitudinalPersonality", self.personality_profile)
# self.previous_personality_profile = self.personality_profile
# # Toggle Experimental Mode from steering wheel function
# if self.experimental_mode_via_lkas and ret.cruiseState.available:
# lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"]
# if lkas_pressed and not self.lkas_previously_pressed:
# if self.conditional_experimental_mode:
# # Set "CEStatus" to work with "Conditional Experimental Mode"
# conditional_status = self.param_memory.get_int("CEStatus")
# override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2
# self.param_memory.put_int("CEStatus", override_value)
# else:
# experimental_mode = self.param.get_bool("ExperimentalMode")
# # Invert the value of "ExperimentalMode"
# put_bool_nonblocking("ExperimentalMode", not experimental_mode)
# self.lkas_previously_pressed = lkas_pressed
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)
ret = car.CarState.new_message()
self.is_metric = cp.vl["CRUISE_BUTTONS_ALT"]["DISTANCE_UNIT"] != 1
speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
if self.CP.carFingerprint in (EV_CAR | HYBRID_CAR):
offset = 255. if self.CP.carFingerprint in EV_CAR else 1023.
ret.gas = cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL"] / offset
ret.gasPressed = ret.gas > 1e-5
else:
ret.gasPressed = bool(cp.vl[self.accelerator_msg_canfd]["ACCELERATOR_PEDAL_PRESSED"])
ret.brakePressed = cp.vl["TCS"]["DriverBraking"] == 1
ret.doorOpen = cp.vl["DOORS_SEATBELTS"]["DRIVER_DOOR"] == 1
ret.seatbeltUnlatched = cp.vl["DOORS_SEATBELTS"]["DRIVER_SEATBELT"] == 0
gear = cp.vl[self.gear_msg_canfd]["GEAR"]
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
# TODO: figure out positions
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_1"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_2"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_3"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_4"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"]
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"]
ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)
ret.steerFaultTemporary = cp.vl["MDPS"]["LKA_FAULT"] != 0
# TODO: alt signal usage may be described by cp.vl['BLINKERS']['USE_ALT_LAMP']
left_blinker_sig, right_blinker_sig = "LEFT_LAMP", "RIGHT_LAMP"
if self.CP.carFingerprint == CAR.KONA_EV_2ND_GEN:
left_blinker_sig, right_blinker_sig = "LEFT_LAMP_ALT", "RIGHT_LAMP_ALT"
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"][left_blinker_sig],
cp.vl["BLINKERS"][right_blinker_sig])
if self.CP.enableBsm:
ret.leftBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FL_INDICATOR"] != 0
ret.rightBlindspot = cp.vl["BLINDSPOTS_REAR_CORNERS"]["FR_INDICATOR"] != 0
# cruise state
# CAN FD cars enable on main button press, set available if no TCS faults preventing engagement
ret.cruiseState.available = self.main_enabled
if self.CP.openpilotLongitudinalControl:
# These are not used for engage/disengage since openpilot keeps track of state using the buttons
ret.cruiseState.enabled = cp.vl["TCS"]["ACC_REQ"] == 1
ret.cruiseState.standstill = False
else:
cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp
ret.cruiseState.enabled = cp_cruise_info.vl["SCC_CONTROL"]["ACCMode"] in (1, 2)
ret.cruiseState.standstill = cp_cruise_info.vl["SCC_CONTROL"]["CRUISE_STANDSTILL"] == 1
ret.cruiseState.speed = cp_cruise_info.vl["SCC_CONTROL"]["VSetDis"] * speed_factor
self.cruise_info = copy.copy(cp_cruise_info.vl["SCC_CONTROL"])
# Manual Speed Limit Assist is a feature that replaces non-adaptive cruise control on EV CAN FD platforms.
# It limits the vehicle speed, overridable by pressing the accelerator past a certain point.
# The car will brake, but does not respect positive acceleration commands in this mode
# TODO: find this message on ICE & HYBRID cars + cruise control signals (if exists)
if self.CP.carFingerprint in EV_CAR:
ret.cruiseState.nonAdaptive = cp.vl["MANUAL_SPEED_LIMIT_ASSIST"]["MSLA_ENABLED"] == 1
self.prev_cruise_buttons = self.cruise_buttons[-1]
self.cruise_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["CRUISE_BUTTONS"])
self.prev_main_buttons = self.main_buttons[-1]
self.main_buttons.extend(cp.vl_all[self.cruise_btns_msg_canfd]["ADAPTIVE_CRUISE_MAIN_BTN"])
if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
self.main_enabled = not self.main_enabled
self.buttons_counter = cp.vl[self.cruise_btns_msg_canfd]["COUNTER"]
ret.accFaulted = cp.vl["TCS"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
if self.CP.flags & HyundaiFlags.CANFD_HDA2:
self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING
else cp_cam.vl["CAM_0x2a4"])
SpeedLimitController.load_state()
SpeedLimitController.car_speed_limit = self.calculate_speed_limit_canfd(self.CP, cp, cp_cam) * speed_factor
SpeedLimitController.write_car_state()
# self.custom_speed_up = False
self.oscar_lane_center_btn_pressed = False
if ret.cruiseState.available:
if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
self.oscar_lane_center_btn_pressed = True
lkas_pressed = False
try:
lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"]
except:
nothing = 0
# intentionally cause a failure
if lkas_pressed:
floog=norpdywoop
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.oscar_lane_center_btn_pressed= True
# Driving personalities function
# if self.personalities_via_wheel and ret.cruiseState.available:
# # Sync with the onroad UI button
# if self.param_memory.get_bool("PersonalityChangedViaUI"):
# self.personality_profile = self.param.get_int("LongitudinalPersonality")
# self.param_memory.put_bool("PersonalityChangedViaUI", False)
# # Change personality upon steering wheel button press
# if self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0:
# self.param_memory.put_bool("PersonalityChangedViaWheel", True)
# self.personality_profile = (self.previous_personality_profile + 2) % 3
# if self.personality_profile != self.previous_personality_profile and self.personality_profile >= 0:
# self.param.put_int("LongitudinalPersonality", self.personality_profile)
# self.previous_personality_profile = self.personality_profile
# # Toggle Experimental Mode from steering wheel function
# if self.experimental_mode_via_lkas and ret.cruiseState.available:
# lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"]
# if lkas_pressed and not self.lkas_previously_pressed:
# if self.conditional_experimental_mode:
# # Set "CEStatus" to work with "Conditional Experimental Mode"
# conditional_status = self.param_memory.get_int("CEStatus")
# override_value = 0 if conditional_status in (1, 2, 3, 4) else 1 if conditional_status >= 5 else 2
# self.param_memory.put_int("CEStatus", override_value)
# else:
# experimental_mode = self.param.get_bool("ExperimentalMode")
# # Invert the value of "ExperimentalMode"
# put_bool_nonblocking("ExperimentalMode", not experimental_mode)
# self.lkas_previously_pressed = lkas_pressed
Watcher.log_watch("hyundai_carstate_update_canfd_self", self)
return ret
# BBOT does not work
def calculate_speed_limit_canfd(self, CP, cp, cp_cam):
try:
self._speed_limit_clu = cp.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"]
return self._speed_limit_clu if self._speed_limit_clu not in (0, 255) else 0
except:
return 0
def get_can_parser(self, CP):
if CP.carFingerprint in CANFD_CAR:
return self.get_can_parser_canfd(CP)
messages = [
# address, frequency
("MDPS12", 50),
("TCS13", 50),
("TCS15", 10),
("CLU11", 50),
("CLU15", 5),
("ESP12", 100),
("CGW1", 10),
("CGW2", 5),
("CGW4", 5),
("WHL_SPD11", 50),
("SAS11", 100),
]
if not CP.openpilotLongitudinalControl and CP.carFingerprint not in CAMERA_SCC_CAR:
messages += [
("SCC11", 50),
("SCC12", 50),
]
if CP.flags & HyundaiFlags.USE_FCA.value:
messages.append(("FCA11", 50))
if CP.enableBsm:
messages.append(("LCA11", 50))
if CP.carFingerprint in (HYBRID_CAR | EV_CAR):
messages.append(("E_EMS11", 50))
else:
messages += [
("EMS12", 100),
("EMS16", 100),
]
if CP.carFingerprint in (HYBRID_CAR | EV_CAR):
messages.append(("ELECT_GEAR", 20))
elif CP.carFingerprint in CAN_GEARS["use_cluster_gears"]:
pass
elif CP.carFingerprint in CAN_GEARS["use_tcu_gears"]:
messages.append(("TCU12", 100))
else:
messages.append(("LVR12", 100))
messages.append(("BCM_PO_11", 50))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
@staticmethod
def get_cam_can_parser(CP):
if CP.carFingerprint in CANFD_CAR:
return CarState.get_cam_can_parser_canfd(CP)
messages = [
("LKAS11", 100)
]
if not CP.openpilotLongitudinalControl and CP.carFingerprint in CAMERA_SCC_CAR:
messages += [
("SCC11", 50),
("SCC12", 50),
]
if CP.flags & HyundaiFlags.USE_FCA.value:
messages.append(("FCA11", 50))
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
def get_can_parser_canfd(self, CP):
messages = [
(self.gear_msg_canfd, 100),
(self.accelerator_msg_canfd, 100),
@@ -485,7 +83,7 @@ class CarState(CarStateBase):
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).ECAN)
@staticmethod
def get_cam_can_parser_canfd(CP):
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"
@@ -495,6 +93,4 @@ class CarState(CarStateBase):
("SCC_CONTROL", 50),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).CAM)

224
selfdrive/car/hyundai/hyundaicanfd.org Normal file
View File

@@ -0,0 +1,224 @@
from openpilot.common.numpy_fast import clip
from openpilot.selfdrive.car import CanBusBase
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags
class CanBus(CanBusBase):
def __init__(self, CP, hda2=None, fingerprint=None) -> None:
super().__init__(CP, fingerprint)
if hda2 is None:
assert CP is not None
hda2 = CP.flags & HyundaiFlags.CANFD_HDA2.value
# On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. HDA2 cars
# have a different harness than the HDA1 and non-HDA variants in order to split
# a different bus, since the steering is done by different ECUs.
self._a, self._e = 1, 0
if hda2:
self._a, self._e = 0, 1
self._a += self.offset
self._e += self.offset
self._cam = 2 + self.offset
@property
def ECAN(self):
return self._e
@property
def ACAN(self):
return self._a
@property
def CAM(self):
return self._cam
def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer):
ret = []
values = {
"LKA_MODE": 2,
"LKA_ICON": 2 if enabled else 1 if lat_active else 0,
"TORQUE_REQUEST": apply_steer,
"LKA_ASSIST": 0,
"STEER_REQ": 1 if lat_active else 0,
"STEER_MODE": 0,
"HAS_LANE_SAFETY": 0, # hide LKAS settings
"NEW_SIGNAL_1": 0,
"NEW_SIGNAL_2": 0,
}
if CP.flags & HyundaiFlags.CANFD_HDA2:
hda2_lkas_msg = "LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else "LKAS"
if CP.openpilotLongitudinalControl:
ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
ret.append(packer.make_can_msg(hda2_lkas_msg, CAN.ACAN, values))
else:
ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
return ret
def create_suppress_lfa(packer, CAN, hda2_lfa_block_msg, hda2_alt_steering):
suppress_msg = "CAM_0x362" if hda2_alt_steering else "CAM_0x2a4"
msg_bytes = 32 if hda2_alt_steering else 24
values = {f"BYTE{i}": hda2_lfa_block_msg[f"BYTE{i}"] for i in range(3, msg_bytes) if i != 7}
values["COUNTER"] = hda2_lfa_block_msg["COUNTER"]
values["SET_ME_0"] = 0
values["SET_ME_0_2"] = 0
values["LEFT_LANE_LINE"] = 0
values["RIGHT_LANE_LINE"] = 0
return packer.make_can_msg(suppress_msg, CAN.ACAN, values)
def create_buttons(packer, CP, CAN, cnt, btn):
values = {
"COUNTER": cnt,
"SET_ME_1": 1,
"CRUISE_BUTTONS": btn,
}
bus = CAN.ECAN if CP.flags & HyundaiFlags.CANFD_HDA2 else CAN.CAM
return packer.make_can_msg("CRUISE_BUTTONS", bus, values)
def create_acc_cancel(packer, CP, CAN, cruise_info_copy):
# TODO: why do we copy different values here?
if CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value:
values = {s: cruise_info_copy[s] for s in [
"COUNTER",
"CHECKSUM",
"NEW_SIGNAL_1",
"MainMode_ACC",
"ACCMode",
"ZEROS_9",
"CRUISE_STANDSTILL",
"ZEROS_5",
"DISTANCE_SETTING",
"VSetDis",
]}
else:
values = {s: cruise_info_copy[s] for s in [
"COUNTER",
"CHECKSUM",
"ACCMode",
"VSetDis",
"CRUISE_STANDSTILL",
]}
values.update({
"ACCMode": 4,
"aReqRaw": 0.0,
"aReqValue": 0.0,
})
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_lfahda_cluster(packer, CAN, enabled):
values = {
"HDA_ICON": 1 if enabled else 0,
# 0 off, 1 gray, 2 green, 3 blinking (wheel icon)
"LFA_ICON": 0 if enabled else 0, # was green before i think in 2? 3 should be flashing?
# "LFA_ICON": 2 if enabled else 0,
}
return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)
def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, personality):
jerk = 5
jn = jerk / 50
if not enabled or gas_override:
a_val, a_raw = 0, 0
else:
a_raw = accel
a_val = clip(accel, accel_last - jn, accel_last + jn)
values = {
"ACCMode": 0 if not enabled else (2 if gas_override else 1),
"MainMode_ACC": 1,
"StopReq": 1 if stopping else 0,
"aReqValue": a_val,
"aReqRaw": a_raw,
"VSetDis": set_speed,
"JerkLowerLimit": jerk if enabled else 1,
"JerkUpperLimit": 3.0,
"ACC_ObjDist": 1,
"ObjValid": 0,
"OBJ_STATUS": 2,
"SET_ME_2": 0x4,
"SET_ME_3": 0x3,
"SET_ME_TMP_64": 0x64,
"DISTANCE_SETTING": personality + 1,
}
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_spas_messages(packer, CAN, frame, left_blink, right_blink):
ret = []
values = {
}
ret.append(packer.make_can_msg("SPAS1", CAN.ECAN, values))
blink = 0
if left_blink:
blink = 3
elif right_blink:
blink = 4
values = {
"BLINKER_CONTROL": blink,
}
ret.append(packer.make_can_msg("SPAS2", CAN.ECAN, values))
return ret
def create_adrv_messages(packer, CAN, frame):
# messages needed to car happy after disabling
# the ADAS Driving ECU to do longitudinal control
ret = []
values = {
}
ret.append(packer.make_can_msg("ADRV_0x51", CAN.ACAN, values))
if frame % 2 == 0:
values = {
'AEB_SETTING': 0x1, # show AEB disabled icon
'SET_ME_2': 0x2,
'SET_ME_FF': 0xff,
'SET_ME_FC': 0xfc,
'SET_ME_9': 0x9,
}
ret.append(packer.make_can_msg("ADRV_0x160", CAN.ECAN, values))
if frame % 5 == 0:
values = {
'SET_ME_1C': 0x1c,
'SET_ME_FF': 0xff,
'SET_ME_TMP_F': 0xf,
'SET_ME_TMP_F_2': 0xf,
}
ret.append(packer.make_can_msg("ADRV_0x1ea", CAN.ECAN, values))
values = {
'SET_ME_E1': 0xe1,
'SET_ME_3A': 0x3a,
}
ret.append(packer.make_can_msg("ADRV_0x200", CAN.ECAN, values))
if frame % 20 == 0:
values = {
'SET_ME_15': 0x15,
}
ret.append(packer.make_can_msg("ADRV_0x345", CAN.ECAN, values))
if frame % 100 == 0:
values = {
'SET_ME_22': 0x22,
'SET_ME_41': 0x41,
}
ret.append(packer.make_can_msg("ADRV_0x1da", CAN.ECAN, values))
return ret