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

Always On Lateral

Browse Source 
Added toggle for Always On Lateral / No disengage lateral on gas and brake.

Lots of credit goes to "pfeiferj"! Couldn't of done it without him!

https: //github.com/pfeiferj/openpilot
Co-Authored-By: Jacob Pfeifer <jacob@pfeifer.dev>
This commit is contained in:
FrogAi
2024-02-27 16:34:47 -07:00
parent a76689801d
commit a6aa132b3d
41 changed files with 170 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
selfdrive/car/chrysler/carcontroller.py
View File

@@ -42,7 +42,7 @@ class CarController:
if self.frame % 25 == 0:
if CS.lkas_car_model != -1:
can_sends.append(chryslercan.create_lkas_hud(self.packer, self.CP, lkas_active, CC.hudControl.visualAlert,
self.hud_count, CS.lkas_car_model, CS.auto_high_beam))
self.hud_count, CS.lkas_car_model, CS.auto_high_beam, CC.latActive))
self.hud_count += 1
# steering

5
selfdrive/car/chrysler/chryslercan.py
View File

@@ -4,7 +4,7 @@ from openpilot.selfdrive.car.chrysler.values import RAM_CARS
GearShifter = car.CarState.GearShifter
VisualAlert = car.CarControl.HUDControl.VisualAlert
def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam):
def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam, lat_active):
# LKAS_HUD - Controls what lane-keeping icon is displayed
# == Color ==
@@ -27,7 +27,7 @@ def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, au
# 7 Normal
# 6 lane departure place hands on wheel
color = 2 if lkas_active else 1
color = 2 if lkas_active else 1 if lat_active else 0
lines = 3 if lkas_active else 0
alerts = 7 if lkas_active else 0
@@ -48,6 +48,7 @@ def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, au
if CP.carFingerprint in RAM_CARS:
values['AUTO_HIGH_BEAM_ON'] = auto_high_beam
values['LKAS_DISABLED'] = 0 if lat_active else 1
return packer.make_can_msg("DAS_6", 0, values)

2
selfdrive/car/honda/carcontroller.py
View File

@@ -253,7 +253,7 @@ class CarController:
if self.frame % 10 == 0:
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible,
hud_control.lanesVisible, fcw_display, acc_alert, steer_required)
can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud))
can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud, CC.latActive))
if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH:
self.speed = pcm_speed

4
selfdrive/car/honda/hondacan.py
View File

@@ -116,7 +116,7 @@ def create_bosch_supplemental_1(packer, car_fingerprint):
return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values)
def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud):
def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud, lat_active):
commands = []
bus_pt = get_pt_bus(CP.carFingerprint)
radar_disabled = CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl
@@ -149,7 +149,7 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud,
lkas_hud_values = {
'SET_ME_X41': 0x41,
'STEERING_REQUIRED': hud.steer_required,
'SOLID_LANES': hud.lanes_visible,
'SOLID_LANES': lat_active,
'BEEP': 0,
}

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

@@ -120,7 +120,7 @@ class CarController:
# 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))
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled, CC.latActive))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
@@ -151,11 +151,11 @@ class CarController:
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))
CC.cruiseControl.override, use_fca, CS.out.cruiseState.available))
# 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))
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled, CC.latActive))
# 5 Hz ACC options
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:

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

@@ -53,6 +53,7 @@ class CarState(CarStateBase):
self.params = CarControllerParams(CP)
# FrogPilot variables
self.main_enabled = False
def update(self, cp, cp_cam, frogpilot_variables):
if self.CP.carFingerprint in CANFD_CAR:
@@ -104,7 +105,7 @@ class CarState(CarStateBase):
# 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 = cp.vl["TCS13"]["ACCEnable"] == 0
ret.cruiseState.available = self.main_enabled
ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
ret.cruiseState.standstill = False
ret.cruiseState.nonAdaptive = False
@@ -164,7 +165,10 @@ class CarState(CarStateBase):
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
return ret
@@ -219,7 +223,7 @@ class CarState(CarStateBase):
# cruise state
# CAN FD cars enable on main button press, set available if no TCS faults preventing engagement
ret.cruiseState.available = cp.vl["TCS"]["ACCEnable"] == 0
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
@@ -240,7 +244,10 @@ class CarState(CarStateBase):
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

8
selfdrive/car/hyundai/hyundaican.py
View File

@@ -117,20 +117,20 @@ def create_clu11(packer, frame, clu11, button, car_fingerprint):
return packer.make_can_msg("CLU11", bus, values)
def create_lfahda_mfc(packer, enabled, hda_set_speed=0):
def create_lfahda_mfc(packer, enabled, lat_active, hda_set_speed=0):
values = {
"LFA_Icon_State": 2 if enabled else 0,
"LFA_Icon_State": 2 if lat_active else 0,
"HDA_Active": 1 if hda_set_speed else 0,
"HDA_Icon_State": 2 if hda_set_speed else 0,
"HDA_VSetReq": hda_set_speed,
}
return packer.make_can_msg("LFAHDA_MFC", 0, values)
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca):
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca, cruise_available):
commands = []
scc11_values = {
"MainMode_ACC": 1,
"MainMode_ACC": 1 if cruise_available else 0,
"TauGapSet": 4,
"VSetDis": set_speed if enabled else 0,
"AliveCounterACC": idx % 0x10,

6
selfdrive/car/hyundai/hyundaicanfd.py
View File

@@ -41,7 +41,7 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer):
values = {
"LKA_MODE": 2,
"LKA_ICON": 2 if enabled else 1,
"LKA_ICON": 2 if lat_active else 1,
"TORQUE_REQUEST": apply_steer,
"LKA_ASSIST": 0,
"STEER_REQ": 1 if lat_active else 0,
@@ -113,10 +113,10 @@ def create_acc_cancel(packer, CP, CAN, cruise_info_copy):
})
return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values)
def create_lfahda_cluster(packer, CAN, enabled):
def create_lfahda_cluster(packer, CAN, enabled, lat_active):
values = {
"HDA_ICON": 1 if enabled else 0,
"LFA_ICON": 2 if enabled else 0,
"LFA_ICON": 2 if lat_active else 0,
}
return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)

2
selfdrive/car/nissan/carcontroller.py
View File

@@ -67,7 +67,7 @@ class CarController:
if self.CP.carFingerprint != CAR.ALTIMA:
if self.frame % 2 == 0:
can_sends.append(nissancan.create_lkas_hud_msg(self.packer, CS.lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible,
hud_control.leftLaneDepart, hud_control.rightLaneDepart))
hud_control.leftLaneDepart, hud_control.rightLaneDepart, CC.latActive))
if self.frame % 50 == 0:
can_sends.append(nissancan.create_lkas_hud_info_msg(

8
selfdrive/car/nissan/nissancan.py
View File

@@ -65,7 +65,7 @@ def create_cancel_msg(packer, cancel_msg, cruise_cancel):
return packer.make_can_msg("CANCEL_MSG", 2, values)
def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart):
def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, left_lane_depart, right_lane_depart, lat_active):
values = {s: lkas_hud_msg[s] for s in [
"LARGE_WARNING_FLASHING",
"SIDE_RADAR_ERROR_FLASHING1",
@@ -98,9 +98,9 @@ def create_lkas_hud_msg(packer, lkas_hud_msg, enabled, left_line, right_line, le
values["RIGHT_LANE_YELLOW_FLASH"] = 1 if right_lane_depart else 0
values["LEFT_LANE_YELLOW_FLASH"] = 1 if left_lane_depart else 0
values["LARGE_STEERING_WHEEL_ICON"] = 2 if enabled else 0
values["RIGHT_LANE_GREEN"] = 1 if right_line and enabled else 0
values["LEFT_LANE_GREEN"] = 1 if left_line and enabled else 0
values["LARGE_STEERING_WHEEL_ICON"] = 2 if lat_active else 0
values["RIGHT_LANE_GREEN"] = 1 if right_line and lat_active else 0
values["LEFT_LANE_GREEN"] = 1 if left_line and lat_active else 0
return packer.make_can_msg("PROPILOT_HUD", 0, values)

2
selfdrive/car/subaru/carcontroller.py
View File

@@ -100,7 +100,7 @@ class CarController:
can_sends.append(subarucan.create_es_lkas_state(self.packer, self.frame // 10, CS.es_lkas_state_msg, CC.enabled, hud_control.visualAlert,
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
hud_control.leftLaneDepart, hud_control.rightLaneDepart))
hud_control.leftLaneDepart, hud_control.rightLaneDepart, CC.latActive))
if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT:
can_sends.append(subarucan.create_es_infotainment(self.packer, self.frame // 10, CS.es_infotainment_msg, hud_control.visualAlert))

6
selfdrive/car/subaru/subarucan.py
View File

@@ -66,7 +66,7 @@ def create_es_distance(packer, frame, es_distance_msg, bus, pcm_cancel_cmd, long
return packer.make_can_msg("ES_Distance", bus, values)
def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart, lat_active):
values = {s: es_lkas_state_msg[s] for s in [
"CHECKSUM",
"LKAS_Alert_Msg",
@@ -118,9 +118,11 @@ def create_es_lkas_state(packer, frame, es_lkas_state_msg, enabled, visual_alert
elif right_lane_depart:
values["LKAS_Alert"] = 11 # Right lane departure dash alert
if enabled:
if lat_active:
values["LKAS_ACTIVE"] = 1 # Show LKAS lane lines
values["LKAS_Dash_State"] = 2 # Green enabled indicator
values["LKAS_Left_Line_Enable"] = 1
values["LKAS_Right_Line_Enable"] = 1
else:
values["LKAS_Dash_State"] = 0 # LKAS Not enabled

2
selfdrive/car/toyota/carcontroller.py
View File

@@ -179,7 +179,7 @@ class CarController:
if self.frame % 20 == 0 or send_ui:
can_sends.append(toyotacan.create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible,
hud_control.rightLaneVisible, hud_control.leftLaneDepart,
hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud))
hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud, lat_active))
if (self.frame % 100 == 0 or send_ui) and (self.CP.enableDsu or self.CP.flags & ToyotaFlags.DISABLE_RADAR.value):
can_sends.append(toyotacan.create_fcw_command(self.packer, fcw_alert))

6
selfdrive/car/toyota/toyotacan.py
View File

@@ -73,12 +73,12 @@ def create_fcw_command(packer, fcw):
return packer.make_can_msg("PCS_HUD", 0, values)
def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled, stock_lkas_hud):
def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled, stock_lkas_hud, lat_active):
values = {
"TWO_BEEPS": chime,
"LDA_ALERT": steer,
"RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2,
"LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2,
"RIGHT_LINE": 0 if not lat_active else 3 if right_lane_depart else 1 if right_line else 2,
"LEFT_LINE": 0 if not lat_active else 3 if left_lane_depart else 1 if left_line else 2,
"BARRIERS": 1 if enabled else 0,
# static signals

2
selfdrive/car/volkswagen/carcontroller.py
View File

@@ -94,7 +94,7 @@ class CarController:
if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw):
hud_alert = self.CCP.LDW_MESSAGES["laneAssistTakeOver"]
can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.enabled,
CS.out.steeringPressed, hud_alert, hud_control))
CS.out.steeringPressed, hud_alert, hud_control, CC.latActive))
if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl:
lead_distance = 0

6
selfdrive/car/volkswagen/mqbcan.py
View File

@@ -28,7 +28,7 @@ def create_eps_update(packer, bus, eps_stock_values, ea_simulated_torque):
return packer.make_can_msg("LH_EPS_03", bus, values)
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control):
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control, lat_active):
values = {}
if len(ldw_stock_values):
values = {s: ldw_stock_values[s] for s in [
@@ -40,8 +40,8 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pres
]}
values.update({
"LDW_Status_LED_gelb": 1 if enabled and steering_pressed else 0,
"LDW_Status_LED_gruen": 1 if enabled and not steering_pressed else 0,
"LDW_Status_LED_gelb": 1 if lat_active and steering_pressed else 0,
"LDW_Status_LED_gruen": 1 if lat_active and not steering_pressed else 0,
"LDW_Lernmodus_links": 3 if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible,
"LDW_Lernmodus_rechts": 3 if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible,
"LDW_Texte": hud_alert,

6
selfdrive/car/volkswagen/pqcan.py
View File

@@ -9,7 +9,7 @@ def create_steering_control(packer, bus, apply_steer, lkas_enabled):
return packer.make_can_msg("HCA_1", bus, values)
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control):
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control, lat_active):
values = {}
if len(ldw_stock_values):
values = {s: ldw_stock_values[s] for s in [
@@ -21,8 +21,8 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pres
]}
values.update({
"LDW_Lampe_gelb": 1 if enabled and steering_pressed else 0,
"LDW_Lampe_gruen": 1 if enabled and not steering_pressed else 0,
"LDW_Lampe_gelb": 1 if lat_active and steering_pressed else 0,
"LDW_Lampe_gruen": 1 if lat_active and not steering_pressed else 0,
"LDW_Lernmodus_links": 3 if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible,
"LDW_Lernmodus_rechts": 3 if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible,
"LDW_Textbits": hud_alert,